1 // Copyright ( c ) 2013, Open Source Robotics Foundation. All rights reserved.
// Copyright ( c ) 2013, The Johns Hopkins University. All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// * Neither the name of the Open Source Robotics Foundation nor the names of its
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES ( INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE )
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
/* Author: Dave Coleman, Jonathan Bohren
Desc: Gazebo plugin for ros_control that allows 'hardware_interfaces' to be plugged in
using pluginlib
*/
#ifndef GAZEBO_ROS2_CONTROL__GAZEBO_ROS2_CONTROL_PLUGIN_HPP_
#define GAZEBO_ROS2_CONTROL__GAZEBO_ROS2_CONTROL_PLUGIN_HPP_
#include <memory>
#include <string>
#include <vector>
#include "controller_manager/controller_manager.hpp"
#include "gazebo/common/common.hh"
#include "gazebo/physics/Model.hh"
namespace gazebo_ros2_control
{
49 class GazeboRosControlPrivate;
51 class GazeboRosControlPlugin : public gazebo::ModelPlugin
{
public:
54 GazeboRosControlPlugin( );
56 ~GazeboRosControlPlugin( );
// Overloaded Gazebo entry point
void Load( gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf ) override;
private:
/// Private data pointer
std::unique_ptr<GazeboRosControlPrivate> impl_;
};
} // namespace gazebo_ros2_control
#endif // GAZEBO_ROS2_CONTROL__GAZEBO_ROS2_CONTROL_PLUGIN_HPP_
1 // Copyright 2021 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_HPP_
#define GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_HPP_
#include <memory>
#include <string>
#include <vector>
#include "angles/angles.h"
#include "gazebo_ros2_control/gazebo_system_interface.hpp"
#include "std_msgs/msg/bool.hpp"
namespace gazebo_ros2_control
{
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
// Forward declaration
34 class GazeboSystemPrivate;
// These class must inherit `gazebo_ros2_control::GazeboSystemInterface` which implements a
// simulated `ros2_control` `hardware_interface::SystemInterface`.
39 class GazeboSystem : public GazeboSystemInterface
{
public:
// Documentation Inherited
CallbackReturn on_init( const hardware_interface::HardwareInfo & system_info )
override;
// Documentation Inherited
std::vector<hardware_interface::StateInterface> export_state_interfaces( ) override;
// Documentation Inherited
std::vector<hardware_interface::CommandInterface> export_command_interfaces( ) override;
// Documentation Inherited
CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state ) override;
// Documentation Inherited
CallbackReturn on_deactivate( const rclcpp_lifecycle::State & previous_state ) override;
// Documentation Inherited
hardware_interface::return_type read(
const rclcpp::Time & time,
const rclcpp::Duration & period ) override;
// Documentation Inherited
hardware_interface::return_type write(
const rclcpp::Time & time,
const rclcpp::Duration & period ) override;
// Documentation Inherited
bool initSim(
rclcpp::Node::SharedPtr & model_nh,
gazebo::physics::ModelPtr parent_model,
const hardware_interface::HardwareInfo & hardware_info,
sdf::ElementPtr sdf ) override;
private:
void registerJoints(
const hardware_interface::HardwareInfo & hardware_info,
gazebo::physics::ModelPtr parent_model );
void registerSensors(
const hardware_interface::HardwareInfo & hardware_info,
gazebo::physics::ModelPtr parent_model );
/// \brief Private data class
std::unique_ptr<GazeboSystemPrivate> dataPtr;
};
} // namespace gazebo_ros2_control
#endif // GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_HPP_
1 // Copyright 2021 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_INTERFACE_HPP_
#define GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_INTERFACE_HPP_
#include <memory>
#include <string>
#include <vector>
#include "gazebo/physics/Joint.hh"
#include "gazebo/physics/Model.hh"
#include "gazebo/physics/physics.hh"
#include "hardware_interface/system_interface.hpp"
#include "rclcpp/rclcpp.hpp"
namespace gazebo_ros2_control
{
template<class ENUM, class UNDERLYING = typename std::underlying_type<ENUM>::type>
36 class SafeEnum
{
public:
39 SafeEnum( )
: mFlags( 0 ) {}
41 explicit SafeEnum( ENUM singleFlag )
: mFlags( singleFlag ) {}
43 SafeEnum( const SafeEnum & original )
: mFlags( original.mFlags ) {}
46 SafeEnum & operator|=( ENUM addValue ) {mFlags |= addValue; return *this;}
47 SafeEnum operator|( ENUM addValue ) {SafeEnum result( *this ); result |= addValue; return result;}
48 SafeEnum & operator&=( ENUM maskValue ) {mFlags &= maskValue; return *this;}
49 SafeEnum operator&( ENUM maskValue ) {SafeEnum result( *this ); result &= maskValue; return result;}
50 SafeEnum operator~( ) {SafeEnum result( *this ); result.mFlags = ~result.mFlags; return result;}
51 explicit operator bool( ) {return mFlags != 0;}
protected:
54 UNDERLYING mFlags;
};
// SystemInterface provides API-level access to read and command joint properties.
58 class GazeboSystemInterface
59 : public hardware_interface::SystemInterface
{
public:
/// \brief Initilize the system interface
/// param[in] model_nh pointer to the ros2 node
/// param[in] parent_model pointer to the model
/// param[in] control_hardware vector filled with information about robot's control resources
/// param[in] sdf pointer to the SDF
67 virtual bool initSim(
68 rclcpp::Node::SharedPtr & model_nh,
69 gazebo::physics::ModelPtr parent_model,
const hardware_interface::HardwareInfo & hardware_info,
71 sdf::ElementPtr sdf ) = 0;
// Methods used to control a joint.
enum ControlMethod_
{
NONE = 0,
POSITION = ( 1 << 0 ),
VELOCITY = ( 1 << 1 ),
EFFORT = ( 1 << 2 ),
};
typedef SafeEnum<enum ControlMethod_> ControlMethod;
protected:
85 rclcpp::Node::SharedPtr nh_;
};
} // namespace gazebo_ros2_control
#endif // GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_INTERFACE_HPP_
1 // Copyright ( c ) 2013, Open Source Robotics Foundation. All rights reserved.
// Copyright ( c ) 2013, The Johns Hopkins University. All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// * Neither the name of the Open Source Robotics Foundation nor the names of its
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES ( INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE )
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
/* Author: Dave Coleman, Jonathan Bohren
Desc: Gazebo plugin for ros_control that allows 'hardware_interfaces' to be plugged in
using pluginlib
*/
#include <string>
#include <memory>
#include <utility>
#include <vector>
#include "gazebo_ros/node.hpp"
#include "gazebo_ros2_control/gazebo_ros2_control_plugin.hpp"
#include "gazebo_ros2_control/gazebo_system.hpp"
#include "pluginlib/class_loader.hpp"
#include "rclcpp/rclcpp.hpp"
#include "hardware_interface/resource_manager.hpp"
#include "hardware_interface/component_parser.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "yaml-cpp/yaml.h"
using namespace std::chrono_literals;
namespace gazebo_ros2_control
{
60 class GazeboRosControlPrivate
{
public:
GazeboRosControlPrivate( ) = default;
virtual ~GazeboRosControlPrivate( ) = default;
// Called by the world update start event
void Update( );
// Called on world reset
virtual void Reset( );
// Get the URDF XML from the parameter server
std::string getURDF( std::string param_name ) const;
// Node Handles
gazebo_ros::Node::SharedPtr model_nh_;
// Pointer to the model
gazebo::physics::ModelPtr parent_model_;
// Pointer to the update event connection
gazebo::event::ConnectionPtr update_connection_;
// Interface loader
boost::shared_ptr<pluginlib::ClassLoader<
gazebo_ros2_control::GazeboSystemInterface>> robot_hw_sim_loader_;
// String with the robot description
std::string robot_description_;
// String with the name of the node that contains the robot_description
std::string robot_description_node_;
// Name of the file with the controllers configuration
std::string param_file_;
// Executor to spin the controller
rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_;
// Thread where the executor will sping
std::thread thread_executor_spin_;
// Flag to stop the executor thread when this plugin is exiting
bool stop_;
// Controller manager
std::shared_ptr<controller_manager::ControllerManager> controller_manager_;
// Available controllers
std::vector<std::shared_ptr<controller_interface::ControllerInterface>> controllers_;
// Timing
rclcpp::Duration control_period_ = rclcpp::Duration( 1, 0 );
// Last time the update method was called
rclcpp::Time last_update_sim_time_ros_ = rclcpp::Time( ( int64_t )0, RCL_ROS_TIME );
};
120 GazeboRosControlPlugin::GazeboRosControlPlugin( )
: impl_( std::make_unique<GazeboRosControlPrivate>( ) )
{
}
125 GazeboRosControlPlugin::~GazeboRosControlPlugin( )
{
// Stop controller manager thread
impl_->stop_ = true;
impl_->executor_->remove_node( impl_->controller_manager_ );
impl_->executor_->cancel( );
impl_->thread_executor_spin_.join( );
// Disconnect from gazebo events
impl_->update_connection_.reset( );
}
// Overloaded Gazebo entry point
138 void GazeboRosControlPlugin::Load( gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf )
{
RCLCPP_INFO_STREAM(
rclcpp::get_logger( "gazebo_ros2_control" ),
"Loading gazebo_ros2_control plugin" );
// Save pointers to the model
impl_->parent_model_ = parent;
// Get parameters/settings for controllers from ROS param server
// Initialize ROS node
impl_->model_nh_ = gazebo_ros::Node::Get( sdf );
RCLCPP_INFO(
impl_->model_nh_->get_logger( ), "Starting gazebo_ros2_control plugin in namespace: %s",
impl_->model_nh_->get_namespace( ) );
RCLCPP_INFO(
impl_->model_nh_->get_logger( ), "Starting gazebo_ros2_control plugin in ros 2 node: %s",
impl_->model_nh_->get_name( ) );
// Error message if the model couldn't be found
if ( !impl_->parent_model_ ) {
RCLCPP_ERROR_STREAM( impl_->model_nh_->get_logger( ), "parent model is NULL" );
return;
}
// Check that ROS has been initialized
if ( !rclcpp::ok( ) ) {
RCLCPP_FATAL_STREAM(
impl_->model_nh_->get_logger( ),
"A ROS node for Gazebo has not been initialized, unable to load plugin. " <<
"Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package )" );
return;
}
// Get robot_description ROS param name
if ( sdf->HasElement( "robot_param" ) ) {
impl_->robot_description_ = sdf->GetElement( "robot_param" )->Get<std::string>( );
} else {
impl_->robot_description_ = "robot_description"; // default
}
// Get robot_description ROS param name
if ( sdf->HasElement( "robot_param_node" ) ) {
impl_->robot_description_node_ =
sdf->GetElement( "robot_param_node" )->Get<std::string>( );
} else {
impl_->robot_description_node_ = "robot_state_publisher"; // default
}
if ( sdf->HasElement( "parameters" ) ) {
impl_->param_file_ = sdf->GetElement( "parameters" )->Get<std::string>( );
RCLCPP_INFO(
impl_->model_nh_->get_logger( ), "Loading parameter file %s\n", impl_->param_file_.c_str( ) );
} else {
RCLCPP_ERROR(
impl_->model_nh_->get_logger( ), "No parameter file provided. Configuration might be wrong" );
}
// There's currently no direct way to set parameters to the plugin's node
// So we have to parse the plugin file manually and set it to the node's context.
auto rcl_context = impl_->model_nh_->get_node_base_interface( )->get_context( )->get_rcl_context( );
std::vector<std::string> arguments = {"--ros-args", "--params-file", impl_->param_file_.c_str( )};
if ( sdf->HasElement( "ros" ) ) {
sdf = sdf->GetElement( "ros" );
// Set namespace if tag is present
if ( sdf->HasElement( "namespace" ) ) {
std::string ns = sdf->GetElement( "namespace" )->Get<std::string>( );
// prevent exception: namespace must be absolute, it must lead with a '/'
if ( ns.empty( ) || ns[0] != '/' ) {
ns = '/' + ns;
}
std::string ns_arg = std::string( "__ns:=" ) + ns;
arguments.push_back( RCL_REMAP_FLAG );
arguments.push_back( ns_arg );
}
// Get list of remapping rules from SDF
if ( sdf->HasElement( "remapping" ) ) {
sdf::ElementPtr argument_sdf = sdf->GetElement( "remapping" );
arguments.push_back( RCL_ROS_ARGS_FLAG );
while ( argument_sdf ) {
std::string argument = argument_sdf->Get<std::string>( );
arguments.push_back( RCL_REMAP_FLAG );
arguments.push_back( argument );
argument_sdf = argument_sdf->GetNextElement( "remapping" );
}
}
}
std::vector<const char *> argv;
for ( const auto & arg : arguments ) {
argv.push_back( reinterpret_cast<const char *>( arg.data( ) ) );
}
rcl_arguments_t rcl_args = rcl_get_zero_initialized_arguments( );
rcl_ret_t rcl_ret = rcl_parse_arguments(
static_cast<int>( argv.size( ) ),
argv.data( ), rcl_get_default_allocator( ), &rcl_args );
rcl_context->global_arguments = rcl_args;
if ( rcl_ret != RCL_RET_OK ) {
RCLCPP_ERROR( impl_->model_nh_->get_logger( ), "parser error %s\n", rcl_get_error_string( ).str );
rcl_reset_error( );
return;
}
if ( rcl_arguments_get_param_files_count( &rcl_args ) < 1 ) {
RCLCPP_ERROR(
impl_->model_nh_->get_logger( ), "failed to parse yaml file: '%s'\n",
impl_->param_file_.c_str( ) );
return;
}
// Get the Gazebo simulation period
rclcpp::Duration gazebo_period(
std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(
impl_->parent_model_->GetWorld( )->Physics( )->GetMaxStepSize( ) ) ) );
// Read urdf from ros parameter server then
// setup actuators and mechanism control node.
// This call will block if ROS is not properly initialized.
std::string urdf_string;
std::vector<hardware_interface::HardwareInfo> control_hardware;
try {
urdf_string = impl_->getURDF( impl_->robot_description_ );
control_hardware = hardware_interface::parse_control_resources_from_urdf( urdf_string );
} catch ( const std::runtime_error & ex ) {
RCLCPP_ERROR_STREAM(
impl_->model_nh_->get_logger( ),
"Error parsing URDF in gazebo_ros2_control plugin, plugin not active : " << ex.what( ) );
return;
}
std::unique_ptr<hardware_interface::ResourceManager> resource_manager_ =
std::make_unique<hardware_interface::ResourceManager>( );
try {
impl_->robot_hw_sim_loader_.reset(
new pluginlib::ClassLoader<gazebo_ros2_control::GazeboSystemInterface>(
"gazebo_ros2_control",
"gazebo_ros2_control::GazeboSystemInterface" ) );
} catch ( pluginlib::LibraryLoadException & ex ) {
RCLCPP_ERROR(
impl_->model_nh_->get_logger( ), "Failed to create robot simulation interface loader: %s ",
ex.what( ) );
}
for ( unsigned int i = 0; i < control_hardware.size( ); i++ ) {
std::string robot_hw_sim_type_str_ = control_hardware[i].hardware_class_type;
auto gazeboSystem = std::unique_ptr<gazebo_ros2_control::GazeboSystemInterface>(
impl_->robot_hw_sim_loader_->createUnmanagedInstance( robot_hw_sim_type_str_ ) );
rclcpp::Node::SharedPtr node_ros2 = std::dynamic_pointer_cast<rclcpp::Node>( impl_->model_nh_ );
if ( !gazeboSystem->initSim(
node_ros2,
impl_->parent_model_,
control_hardware[i],
sdf ) )
{
RCLCPP_FATAL(
impl_->model_nh_->get_logger( ), "Could not initialize robot simulation interface" );
return;
}
resource_manager_->import_component( std::move( gazeboSystem ), control_hardware[i] );
}
rclcpp_lifecycle::State state(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
hardware_interface::lifecycle_state_names::ACTIVE );
resource_manager_->set_component_state( "GazeboSimSystem", state );
impl_->executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>( );
// Create the controller manager
RCLCPP_INFO( impl_->model_nh_->get_logger( ), "Loading controller_manager" );
impl_->controller_manager_.reset(
new controller_manager::ControllerManager(
std::move( resource_manager_ ),
impl_->executor_,
"controller_manager" ) );
impl_->executor_->add_node( impl_->controller_manager_ );
if ( !impl_->controller_manager_->has_parameter( "update_rate" ) ) {
RCLCPP_ERROR_STREAM(
impl_->model_nh_->get_logger( ), "controller manager doesn't have an update_rate parameter" );
return;
}
auto cm_update_rate = impl_->controller_manager_->get_parameter( "update_rate" ).as_int( );
impl_->control_period_ = rclcpp::Duration(
std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>( 1.0 / static_cast<double>( cm_update_rate ) ) ) );
// Check the period against the simulation period
if ( impl_->control_period_ < gazebo_period ) {
RCLCPP_ERROR_STREAM(
impl_->model_nh_->get_logger( ),
"Desired controller update period ( " << impl_->control_period_.seconds( ) <<
" s ) is faster than the gazebo simulation period ( " <<
gazebo_period.seconds( ) << " s )." );
} else if ( impl_->control_period_ > gazebo_period ) {
RCLCPP_WARN_STREAM(
impl_->model_nh_->get_logger( ),
" Desired controller update period ( " << impl_->control_period_.seconds( ) <<
" s ) is slower than the gazebo simulation period ( " <<
gazebo_period.seconds( ) << " s )." );
}
impl_->stop_ = false;
auto spin = [this]( )
{
while ( rclcpp::ok( ) && !impl_->stop_ ) {
impl_->executor_->spin_once( );
}
};
impl_->thread_executor_spin_ = std::thread( spin );
// Listen to the update event. This event is broadcast every simulation iteration.
impl_->update_connection_ =
gazebo::event::Events::ConnectWorldUpdateBegin(
boost::bind(
&GazeboRosControlPrivate::Update,
impl_.get( ) ) );
RCLCPP_INFO( impl_->model_nh_->get_logger( ), "Loaded gazebo_ros2_control." );
}
// Called by the world update start event
368 void GazeboRosControlPrivate::Update( )
{
// Get the simulation time and period
gazebo::common::Time gz_time_now = parent_model_->GetWorld( )->SimTime( );
rclcpp::Time sim_time_ros( gz_time_now.sec, gz_time_now.nsec, RCL_ROS_TIME );
rclcpp::Duration sim_period = sim_time_ros - last_update_sim_time_ros_;
if ( sim_period >= control_period_ ) {
controller_manager_->read( sim_time_ros, sim_period );
controller_manager_->update( sim_time_ros, sim_period );
last_update_sim_time_ros_ = sim_time_ros;
}
// Always set commands on joints, otherwise at low control frequencies the joints tremble
// as they are updated at a fraction of gazebo sim time
// use same time as for read and update call - this is how it is done is ros2_control_node
controller_manager_->write( sim_time_ros, sim_period );
}
// Called on world reset
388 void GazeboRosControlPrivate::Reset( )
{
// Reset timing variables to not pass negative update periods to controllers on world reset
last_update_sim_time_ros_ = rclcpp::Time( ( int64_t )0, RCL_ROS_TIME );
}
// Get the URDF XML from the parameter server
395 std::string GazeboRosControlPrivate::getURDF( std::string param_name ) const
{
std::string urdf_string;
using namespace std::chrono_literals;
auto parameters_client = std::make_shared<rclcpp::AsyncParametersClient>(
model_nh_, robot_description_node_ );
while ( !parameters_client->wait_for_service( 0.5s ) ) {
if ( !rclcpp::ok( ) ) {
RCLCPP_ERROR(
model_nh_->get_logger( ), "Interrupted while waiting for %s service. Exiting.",
robot_description_node_.c_str( ) );
return 0;
}
RCLCPP_ERROR(
model_nh_->get_logger( ), "%s service not available, waiting again...",
robot_description_node_.c_str( ) );
}
RCLCPP_INFO(
model_nh_->get_logger( ), "connected to service!! %s", robot_description_node_.c_str( ) );
// search and wait for robot_description on param server
while ( urdf_string.empty( ) ) {
std::string search_param_name;
RCLCPP_DEBUG( model_nh_->get_logger( ), "param_name %s", param_name.c_str( ) );
try {
auto f = parameters_client->get_parameters( {param_name} );
f.wait( );
std::vector<rclcpp::Parameter> values = f.get( );
urdf_string = values[0].as_string( );
} catch ( const std::exception & e ) {
RCLCPP_ERROR( model_nh_->get_logger( ), "%s", e.what( ) );
}
if ( !urdf_string.empty( ) ) {
break;
} else {
RCLCPP_ERROR(
model_nh_->get_logger( ), "gazebo_ros2_control plugin is waiting for model"
" URDF in parameter [%s] on the ROS param server.", search_param_name.c_str( ) );
}
usleep( 100000 );
}
RCLCPP_INFO(
model_nh_->get_logger( ), "Recieved urdf from param server, parsing..." );
return urdf_string;
}
// Register this plugin with the simulator
447 GZ_REGISTER_MODEL_PLUGIN( GazeboRosControlPlugin )
} // namespace gazebo_ros2_control
1 // Copyright 2021 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <limits>
#include <map>
#include <memory>
#include <string>
#include <vector>
#include <utility>
#include "gazebo_ros2_control/gazebo_system.hpp"
#include "gazebo/sensors/ImuSensor.hh"
#include "gazebo/sensors/ForceTorqueSensor.hh"
#include "gazebo/sensors/SensorManager.hh"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
struct MimicJoint
{
std::size_t joint_index;
std::size_t mimicked_joint_index;
double multiplier = 1.0;
};
37 class gazebo_ros2_control::GazeboSystemPrivate
{
public:
GazeboSystemPrivate( ) = default;
~GazeboSystemPrivate( ) = default;
/// \brief Degrees od freedom.
size_t n_dof_;
/// \brief Number of sensors.
size_t n_sensors_;
/// \brief Gazebo Model Ptr.
gazebo::physics::ModelPtr parent_model_;
/// \brief last time the write method was called.
rclcpp::Time last_update_sim_time_ros_;
/// \brief vector with the joint's names.
std::vector<std::string> joint_names_;
/// \brief vector with the control method defined in the URDF for each joint.
std::vector<GazeboSystemInterface::ControlMethod> joint_control_methods_;
/// \brief handles to the joints from within Gazebo
std::vector<gazebo::physics::JointPtr> sim_joints_;
/// \brief vector with the current joint position
std::vector<double> joint_position_;
/// \brief vector with the current joint velocity
std::vector<double> joint_velocity_;
/// \brief vector with the current joint effort
std::vector<double> joint_effort_;
/// \brief vector with the current cmd joint position
std::vector<double> joint_position_cmd_;
/// \brief vector with the current cmd joint velocity
std::vector<double> joint_velocity_cmd_;
/// \brief vector with the current cmd joint effort
std::vector<double> joint_effort_cmd_;
/// \brief handles to the imus from within Gazebo
std::vector<gazebo::sensors::ImuSensorPtr> sim_imu_sensors_;
/// \brief An array per IMU with 4 orientation, 3 angular velocity and 3 linear acceleration
std::vector<std::array<double, 10>> imu_sensor_data_;
/// \brief handles to the FT sensors from within Gazebo
std::vector<gazebo::sensors::ForceTorqueSensorPtr> sim_ft_sensors_;
/// \brief An array per FT sensor for 3D force and torquee
std::vector<std::array<double, 6>> ft_sensor_data_;
/// \brief state interfaces that will be exported to the Resource Manager
std::vector<hardware_interface::StateInterface> state_interfaces_;
/// \brief command interfaces that will be exported to the Resource Manager
std::vector<hardware_interface::CommandInterface> command_interfaces_;
/// \brief mapping of mimicked joints to index of joint they mimic
std::vector<MimicJoint> mimic_joints_;
};
namespace gazebo_ros2_control
{
108 bool GazeboSystem::initSim(
109 rclcpp::Node::SharedPtr & model_nh,
110 gazebo::physics::ModelPtr parent_model,
const hardware_interface::HardwareInfo & hardware_info,
112 sdf::ElementPtr sdf )
{
this->dataPtr = std::make_unique<GazeboSystemPrivate>( );
this->dataPtr->last_update_sim_time_ros_ = rclcpp::Time( );
this->nh_ = model_nh;
this->dataPtr->parent_model_ = parent_model;
gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world( )->Physics( );
std::string physics_type_ = physics->GetType( );
if ( physics_type_.empty( ) ) {
RCLCPP_ERROR( this->nh_->get_logger( ), "No physics engine configured in Gazebo." );
return false;
}
registerJoints( hardware_info, parent_model );
registerSensors( hardware_info, parent_model );
if ( this->dataPtr->n_dof_ == 0 && this->dataPtr->n_sensors_ == 0 ) {
RCLCPP_WARN_STREAM( this->nh_->get_logger( ), "There is no joint or sensor available" );
return false;
}
return true;
}
139 void GazeboSystem::registerJoints(
const hardware_interface::HardwareInfo & hardware_info,
141 gazebo::physics::ModelPtr parent_model )
{
this->dataPtr->n_dof_ = hardware_info.joints.size( );
this->dataPtr->joint_names_.resize( this->dataPtr->n_dof_ );
this->dataPtr->joint_control_methods_.resize( this->dataPtr->n_dof_ );
this->dataPtr->joint_position_.resize( this->dataPtr->n_dof_ );
this->dataPtr->joint_velocity_.resize( this->dataPtr->n_dof_ );
this->dataPtr->joint_effort_.resize( this->dataPtr->n_dof_ );
this->dataPtr->joint_position_cmd_.resize( this->dataPtr->n_dof_ );
this->dataPtr->joint_velocity_cmd_.resize( this->dataPtr->n_dof_ );
this->dataPtr->joint_effort_cmd_.resize( this->dataPtr->n_dof_ );
for ( unsigned int j = 0; j < this->dataPtr->n_dof_; j++ ) {
auto & joint_info = hardware_info.joints[j];
std::string joint_name = this->dataPtr->joint_names_[j] = joint_info.name;
gazebo::physics::JointPtr simjoint = parent_model->GetJoint( joint_name );
this->dataPtr->sim_joints_.push_back( simjoint );
if ( !simjoint ) {
RCLCPP_WARN_STREAM(
this->nh_->get_logger( ), "Skipping joint in the URDF named '" << joint_name <<
"' which is not in the gazebo model." );
continue;
}
// Accept this joint and continue configuration
RCLCPP_INFO_STREAM( this->nh_->get_logger( ), "Loading joint: " << joint_name );
std::string suffix = "";
// check if joint is mimicked
if ( joint_info.parameters.find( "mimic" ) != joint_info.parameters.end( ) ) {
const auto mimicked_joint = joint_info.parameters.at( "mimic" );
const auto mimicked_joint_it = std::find_if(
hardware_info.joints.begin( ), hardware_info.joints.end( ),
[&mimicked_joint]( const hardware_interface::ComponentInfo & info ) {
return info.name == mimicked_joint;
} );
if ( mimicked_joint_it == hardware_info.joints.end( ) ) {
throw std::runtime_error(
std::string( "Mimicked joint '" ) + mimicked_joint + "' not found" );
}
MimicJoint mimic_joint;
mimic_joint.joint_index = j;
mimic_joint.mimicked_joint_index = std::distance(
hardware_info.joints.begin( ), mimicked_joint_it );
auto param_it = joint_info.parameters.find( "multiplier" );
if ( param_it != joint_info.parameters.end( ) ) {
mimic_joint.multiplier = std::stod( joint_info.parameters.at( "multiplier" ) );
} else {
mimic_joint.multiplier = 1.0;
}
RCLCPP_INFO_STREAM(
this->nh_->get_logger( ),
"Joint '" << joint_name << "'is mimicing joint '" << mimicked_joint << "' with mutiplier: "
<< mimic_joint.multiplier );
this->dataPtr->mimic_joints_.push_back( mimic_joint );
suffix = "_mimic";
}
RCLCPP_INFO_STREAM( this->nh_->get_logger( ), "\tState:" );
auto get_initial_value = [this]( const hardware_interface::InterfaceInfo & interface_info ) {
if ( !interface_info.initial_value.empty( ) ) {
double value = std::stod( interface_info.initial_value );
RCLCPP_INFO( this->nh_->get_logger( ), "\t\t\t found initial value: %f", value );
return value;
} else {
return 0.0;
}
};
double initial_position = std::numeric_limits<double>::quiet_NaN( );
double initial_velocity = std::numeric_limits<double>::quiet_NaN( );
double initial_effort = std::numeric_limits<double>::quiet_NaN( );
// register the state handles
for ( unsigned int i = 0; i < joint_info.state_interfaces.size( ); i++ ) {
if ( joint_info.state_interfaces[i].name == "position" ) {
RCLCPP_INFO_STREAM( this->nh_->get_logger( ), "\t\t position" );
this->dataPtr->state_interfaces_.emplace_back(
joint_name + suffix,
hardware_interface::HW_IF_POSITION,
&this->dataPtr->joint_position_[j] );
initial_position = get_initial_value( joint_info.state_interfaces[i] );
this->dataPtr->joint_position_[j] = initial_position;
}
if ( joint_info.state_interfaces[i].name == "velocity" ) {
RCLCPP_INFO_STREAM( this->nh_->get_logger( ), "\t\t velocity" );
this->dataPtr->state_interfaces_.emplace_back(
joint_name + suffix,
hardware_interface::HW_IF_VELOCITY,
&this->dataPtr->joint_velocity_[j] );
initial_velocity = get_initial_value( joint_info.state_interfaces[i] );
this->dataPtr->joint_velocity_[j] = initial_velocity;
}
if ( joint_info.state_interfaces[i].name == "effort" ) {
RCLCPP_INFO_STREAM( this->nh_->get_logger( ), "\t\t effort" );
this->dataPtr->state_interfaces_.emplace_back(
joint_name + suffix,
hardware_interface::HW_IF_EFFORT,
&this->dataPtr->joint_effort_[j] );
initial_effort = get_initial_value( joint_info.state_interfaces[i] );
this->dataPtr->joint_effort_[j] = initial_effort;
}
}
RCLCPP_INFO_STREAM( this->nh_->get_logger( ), "\tCommand:" );
// register the command handles
for ( unsigned int i = 0; i < joint_info.command_interfaces.size( ); i++ ) {
if ( joint_info.command_interfaces[i].name == "position" ) {
RCLCPP_INFO_STREAM( this->nh_->get_logger( ), "\t\t position" );
this->dataPtr->joint_control_methods_[j] |= POSITION;
this->dataPtr->command_interfaces_.emplace_back(
joint_name + suffix,
hardware_interface::HW_IF_POSITION,
&this->dataPtr->joint_position_cmd_[j] );
if ( !std::isnan( initial_position ) ) {
this->dataPtr->joint_position_cmd_[j] = initial_position;
}
}
if ( joint_info.command_interfaces[i].name == "velocity" ) {
RCLCPP_INFO_STREAM( this->nh_->get_logger( ), "\t\t velocity" );
this->dataPtr->joint_control_methods_[j] |= VELOCITY;
this->dataPtr->command_interfaces_.emplace_back(
joint_name + suffix,
hardware_interface::HW_IF_VELOCITY,
&this->dataPtr->joint_velocity_cmd_[j] );
if ( !std::isnan( initial_velocity ) ) {
this->dataPtr->joint_velocity_cmd_[j] = initial_velocity;
}
}
if ( joint_info.command_interfaces[i].name == "effort" ) {
this->dataPtr->joint_control_methods_[j] |= EFFORT;
RCLCPP_INFO_STREAM( this->nh_->get_logger( ), "\t\t effort" );
this->dataPtr->command_interfaces_.emplace_back(
joint_name + suffix,
hardware_interface::HW_IF_EFFORT,
&this->dataPtr->joint_effort_cmd_[j] );
if ( !std::isnan( initial_effort ) ) {
this->dataPtr->joint_effort_cmd_[j] = initial_effort;
}
}
}
}
}
290 void GazeboSystem::registerSensors(
const hardware_interface::HardwareInfo & hardware_info,
292 gazebo::physics::ModelPtr parent_model )
{
// Collect gazebo sensor handles
size_t n_sensors = hardware_info.sensors.size( );
std::vector<hardware_interface::ComponentInfo> imu_components_;
std::vector<hardware_interface::ComponentInfo> ft_sensor_components_;
// This is split in two steps: Count the number and type of sensor and associate the interfaces
// So we have resize only once the structures where the data will be stored, and we can safely
// use pointers to the structures
for ( unsigned int j = 0; j < n_sensors; j++ ) {
hardware_interface::ComponentInfo component = hardware_info.sensors[j];
std::string sensor_name = component.name;
// This can't be used, because it only looks for sensor in links, but force_torque_sensor
// must be in a joint, as in not found by SensorScopedName
// std::vector<std::string> gz_sensor_names = parent_model->SensorScopedName( sensor_name );
// Workaround to find sensors whose parent is a link or joint of parent_model
std::vector<std::string> gz_sensor_names;
for ( const auto & s : gazebo::sensors::SensorManager::Instance( )->GetSensors( ) ) {
const std::string sensor_parent = s->ParentName( );
if ( s->Name( ) != sensor_name ) {
continue;
}
if ( ( parent_model->GetJoint( sensor_parent ) != nullptr ) ||
parent_model->GetLink( sensor_parent ) != nullptr )
{
gz_sensor_names.push_back( s->ScopedName( ) );
}
}
if ( gz_sensor_names.empty( ) ) {
RCLCPP_WARN_STREAM(
this->nh_->get_logger( ), "Skipping sensor in the URDF named '" << sensor_name <<
"' which is not in the gazebo model." );
continue;
}
if ( gz_sensor_names.size( ) > 1 ) {
RCLCPP_WARN_STREAM(
this->nh_->get_logger( ), "Sensor in the URDF named '" << sensor_name <<
"' has more than one gazebo sensor with the " <<
"same name, only using the first. It has " << gz_sensor_names.size( ) << " sensors" );
}
gazebo::sensors::SensorPtr simsensor = gazebo::sensors::SensorManager::Instance( )->GetSensor(
gz_sensor_names[0] );
if ( !simsensor ) {
RCLCPP_ERROR_STREAM(
this->nh_->get_logger( ),
"Error retrieving sensor '" << sensor_name << " from the sensor manager" );
continue;
}
if ( simsensor->Type( ) == "imu" ) {
gazebo::sensors::ImuSensorPtr imu_sensor =
std::dynamic_pointer_cast<gazebo::sensors::ImuSensor>( simsensor );
if ( !imu_sensor ) {
RCLCPP_ERROR_STREAM(
this->nh_->get_logger( ),
"Error retrieving casting sensor '" << sensor_name << " to ImuSensor" );
continue;
}
imu_components_.push_back( component );
this->dataPtr->sim_imu_sensors_.push_back( imu_sensor );
} else if ( simsensor->Type( ) == "force_torque" ) {
gazebo::sensors::ForceTorqueSensorPtr ft_sensor =
std::dynamic_pointer_cast<gazebo::sensors::ForceTorqueSensor>( simsensor );
if ( !ft_sensor ) {
RCLCPP_ERROR_STREAM(
this->nh_->get_logger( ),
"Error retrieving casting sensor '" << sensor_name << " to ForceTorqueSensor" );
continue;
}
ft_sensor_components_.push_back( component );
this->dataPtr->sim_ft_sensors_.push_back( ft_sensor );
}
}
this->dataPtr->imu_sensor_data_.resize( this->dataPtr->sim_imu_sensors_.size( ) );
this->dataPtr->ft_sensor_data_.resize( this->dataPtr->sim_ft_sensors_.size( ) );
this->dataPtr->n_sensors_ = this->dataPtr->sim_imu_sensors_.size( ) +
this->dataPtr->sim_ft_sensors_.size( );
for ( unsigned int i = 0; i < imu_components_.size( ); i++ ) {
const std::string & sensor_name = imu_components_[i].name;
RCLCPP_INFO_STREAM( this->nh_->get_logger( ), "Loading sensor: " << sensor_name );
RCLCPP_INFO_STREAM(
this->nh_->get_logger( ), "\tState:" );
for ( const auto & state_interface : imu_components_[i].state_interfaces ) {
static const std::map<std::string, size_t> interface_name_map = {
{"orientation.x", 0},
{"orientation.y", 1},
{"orientation.z", 2},
{"orientation.w", 3},
{"angular_velocity.x", 4},
{"angular_velocity.y", 5},
{"angular_velocity.z", 6},
{"linear_acceleration.x", 7},
{"linear_acceleration.y", 8},
{"linear_acceleration.z", 9},
};
RCLCPP_INFO_STREAM( this->nh_->get_logger( ), "\t\t " << state_interface.name );
size_t data_index = interface_name_map.at( state_interface.name );
this->dataPtr->state_interfaces_.emplace_back(
sensor_name,
state_interface.name,
&this->dataPtr->imu_sensor_data_[i][data_index] );
}
}
for ( unsigned int i = 0; i < ft_sensor_components_.size( ); i++ ) {
const std::string & sensor_name = ft_sensor_components_[i].name;
RCLCPP_INFO_STREAM( this->nh_->get_logger( ), "Loading sensor: " << sensor_name );
RCLCPP_INFO_STREAM(
this->nh_->get_logger( ), "\tState:" );
for ( const auto & state_interface : ft_sensor_components_[i].state_interfaces ) {
static const std::map<std::string, size_t> interface_name_map = {
{"force.x", 0},
{"force.y", 1},
{"force.z", 2},
{"torque.x", 3},
{"torque.y", 4},
{"torque.z", 5}
};
RCLCPP_INFO_STREAM( this->nh_->get_logger( ), "\t\t " << state_interface.name );
size_t data_index = interface_name_map.at( state_interface.name );
this->dataPtr->state_interfaces_.emplace_back(
sensor_name,
state_interface.name,
&this->dataPtr->ft_sensor_data_[i][data_index] );
}
}
}
CallbackReturn
427 GazeboSystem::on_init( const hardware_interface::HardwareInfo & system_info )
{
if ( hardware_interface::SystemInterface::on_init( system_info ) != CallbackReturn::SUCCESS ) {
return CallbackReturn::ERROR;
}
return CallbackReturn::SUCCESS;
}
std::vector<hardware_interface::StateInterface>
436 GazeboSystem::export_state_interfaces( )
{
return std::move( this->dataPtr->state_interfaces_ );
}
std::vector<hardware_interface::CommandInterface>
442 GazeboSystem::export_command_interfaces( )
{
return std::move( this->dataPtr->command_interfaces_ );
}
447 CallbackReturn GazeboSystem::on_activate( const rclcpp_lifecycle::State & previous_state )
{
return CallbackReturn::SUCCESS;
}
452 CallbackReturn GazeboSystem::on_deactivate( const rclcpp_lifecycle::State & previous_state )
{
return CallbackReturn::SUCCESS;
}
457 hardware_interface::return_type GazeboSystem::read(
458 const rclcpp::Time & time,
459 const rclcpp::Duration & period )
{
for ( unsigned int j = 0; j < this->dataPtr->joint_names_.size( ); j++ ) {
if ( this->dataPtr->sim_joints_[j] ) {
this->dataPtr->joint_position_[j] = this->dataPtr->sim_joints_[j]->Position( 0 );
this->dataPtr->joint_velocity_[j] = this->dataPtr->sim_joints_[j]->GetVelocity( 0 );
this->dataPtr->joint_effort_[j] = this->dataPtr->sim_joints_[j]->GetForce( 0u );
}
}
for ( unsigned int j = 0; j < this->dataPtr->sim_imu_sensors_.size( ); j++ ) {
auto sim_imu = this->dataPtr->sim_imu_sensors_[j];
this->dataPtr->imu_sensor_data_[j][0] = sim_imu->Orientation( ).X( );
this->dataPtr->imu_sensor_data_[j][1] = sim_imu->Orientation( ).Y( );
this->dataPtr->imu_sensor_data_[j][2] = sim_imu->Orientation( ).Z( );
this->dataPtr->imu_sensor_data_[j][3] = sim_imu->Orientation( ).W( );
this->dataPtr->imu_sensor_data_[j][4] = sim_imu->AngularVelocity( ).X( );
this->dataPtr->imu_sensor_data_[j][5] = sim_imu->AngularVelocity( ).Y( );
this->dataPtr->imu_sensor_data_[j][6] = sim_imu->AngularVelocity( ).Z( );
this->dataPtr->imu_sensor_data_[j][7] = sim_imu->LinearAcceleration( ).X( );
this->dataPtr->imu_sensor_data_[j][8] = sim_imu->LinearAcceleration( ).Y( );
this->dataPtr->imu_sensor_data_[j][9] = sim_imu->LinearAcceleration( ).Z( );
}
for ( unsigned int j = 0; j < this->dataPtr->sim_ft_sensors_.size( ); j++ ) {
auto sim_ft_sensor = this->dataPtr->sim_ft_sensors_[j];
this->dataPtr->imu_sensor_data_[j][0] = sim_ft_sensor->Force( ).X( );
this->dataPtr->imu_sensor_data_[j][1] = sim_ft_sensor->Force( ).Y( );
this->dataPtr->imu_sensor_data_[j][2] = sim_ft_sensor->Force( ).Z( );
this->dataPtr->imu_sensor_data_[j][3] = sim_ft_sensor->Torque( ).X( );
this->dataPtr->imu_sensor_data_[j][4] = sim_ft_sensor->Torque( ).Y( );
this->dataPtr->imu_sensor_data_[j][5] = sim_ft_sensor->Torque( ).Z( );
}
return hardware_interface::return_type::OK;
}
498 hardware_interface::return_type GazeboSystem::write(
499 const rclcpp::Time & time,
500 const rclcpp::Duration & period )
{
// Get the simulation time and period
gazebo::common::Time gz_time_now = this->dataPtr->parent_model_->GetWorld( )->SimTime( );
rclcpp::Time sim_time_ros( gz_time_now.sec, gz_time_now.nsec );
rclcpp::Duration sim_period = sim_time_ros - this->dataPtr->last_update_sim_time_ros_;
// set values of all mimic joints with respect to mimicked joint
for ( const auto & mimic_joint : this->dataPtr->mimic_joints_ ) {
if ( this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & POSITION &&
this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & POSITION )
{
this->dataPtr->joint_position_cmd_[mimic_joint.joint_index] =
mimic_joint.multiplier *
this->dataPtr->joint_position_cmd_[mimic_joint.mimicked_joint_index];
}
if ( this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & VELOCITY &&
this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & VELOCITY )
{
this->dataPtr->joint_velocity_cmd_[mimic_joint.joint_index] =
mimic_joint.multiplier *
this->dataPtr->joint_velocity_cmd_[mimic_joint.mimicked_joint_index];
}
if ( this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & EFFORT &&
this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & EFFORT )
{
this->dataPtr->joint_effort_cmd_[mimic_joint.joint_index] =
mimic_joint.multiplier * this->dataPtr->joint_effort_cmd_[mimic_joint.mimicked_joint_index];
}
}
for ( unsigned int j = 0; j < this->dataPtr->joint_names_.size( ); j++ ) {
if ( this->dataPtr->sim_joints_[j] ) {
if ( this->dataPtr->joint_control_methods_[j] & POSITION ) {
this->dataPtr->sim_joints_[j]->SetPosition(
0, this->dataPtr->joint_position_cmd_[j],
true );
}
if ( this->dataPtr->joint_control_methods_[j] & VELOCITY ) {
this->dataPtr->sim_joints_[j]->SetVelocity(
0,
this->dataPtr->joint_velocity_cmd_[j] );
}
if ( this->dataPtr->joint_control_methods_[j] & EFFORT ) {
const double effort = this->dataPtr->joint_effort_cmd_[j];
this->dataPtr->sim_joints_[j]->SetForce( 0, effort );
}
}
}
this->dataPtr->last_update_sim_time_ros_ = sim_time_ros;
return hardware_interface::return_type::OK;
}
} // namespace gazebo_ros2_control
#include "pluginlib/class_list_macros.hpp" // NOLINT
557 PLUGINLIB_EXPORT_CLASS(
gazebo_ros2_control::GazeboSystem, gazebo_ros2_control::GazeboSystemInterface )
1 // Copyright 2022 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
using namespace std::chrono_literals;
23 int main( int argc, char * argv[] )
{
rclcpp::init( argc, argv );
std::shared_ptr<rclcpp::Node> node =
std::make_shared<rclcpp::Node>( "diff_drive_test_node" );
auto publisher = node->create_publisher<geometry_msgs::msg::Twist>(
"/diff_drive_base_controller/cmd_vel_unstamped", 10 );
RCLCPP_INFO( node->get_logger( ), "node created" );
geometry_msgs::msg::Twist command;
command.linear.x = 0.2;
command.linear.y = 0.0;
command.linear.z = 0.0;
command.angular.x = 0.0;
command.angular.y = 0.0;
command.angular.z = 0.1;
while ( 1 ) {
publisher->publish( command );
std::this_thread::sleep_for( 50ms );
rclcpp::spin_some( node );
}
rclcpp::shutdown( );
return 0;
}
1 // Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include <string>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
std::shared_ptr<rclcpp::Node> node;
25 int main( int argc, char * argv[] )
{
rclcpp::init( argc, argv );
node = std::make_shared<rclcpp::Node>( "effort_test_node" );
auto publisher = node->create_publisher<std_msgs::msg::Float64MultiArray>(
"/effort_controllers/commands", 10 );
RCLCPP_INFO( node->get_logger( ), "node created" );
std_msgs::msg::Float64MultiArray commands;
using namespace std::chrono_literals;
commands.data.push_back( 0 );
publisher->publish( commands );
std::this_thread::sleep_for( 1s );
commands.data[0] = 100;
publisher->publish( commands );
std::this_thread::sleep_for( 1s );
commands.data[0] = -200;
publisher->publish( commands );
std::this_thread::sleep_for( 1s );
commands.data[0] = 0;
publisher->publish( commands );
std::this_thread::sleep_for( 1s );
rclcpp::shutdown( );
return 0;
}
1 // Copyright 2022 Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include <string>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
std::shared_ptr<rclcpp::Node> node;
25 int main( int argc, char * argv[] )
{
rclcpp::init( argc, argv );
node = std::make_shared<rclcpp::Node>( "gripper_test_node" );
auto publisher = node->create_publisher<std_msgs::msg::Float64MultiArray>(
"/gripper_controller/commands", 10 );
RCLCPP_INFO( node->get_logger( ), "node created" );
std_msgs::msg::Float64MultiArray commands;
using namespace std::chrono_literals;
commands.data.push_back( 0 );
publisher->publish( commands );
std::this_thread::sleep_for( 1s );
commands.data[0] = 0.38;
publisher->publish( commands );
std::this_thread::sleep_for( 1s );
commands.data[0] = 0.19;
publisher->publish( commands );
std::this_thread::sleep_for( 1s );
commands.data[0] = 0;
publisher->publish( commands );
std::this_thread::sleep_for( 1s );
rclcpp::shutdown( );
return 0;
}
1 // Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include <string>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "control_msgs/action/follow_joint_trajectory.hpp"
std::shared_ptr<rclcpp::Node> node;
bool common_goal_accepted = false;
rclcpp_action::ResultCode common_resultcode = rclcpp_action::ResultCode::UNKNOWN;
int common_action_result_code = control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL;
29 void common_goal_response(
rclcpp_action::ClientGoalHandle
<control_msgs::action::FollowJointTrajectory>::SharedPtr goal_handle )
{
RCLCPP_DEBUG(
node->get_logger( ), "common_goal_response time: %f",
rclcpp::Clock( ).now( ).seconds( ) );
if ( !goal_handle ) {
common_goal_accepted = false;
printf( "Goal rejected\n" );
} else {
common_goal_accepted = true;
printf( "Goal accepted\n" );
}
}
45 void common_result_response(
const rclcpp_action::ClientGoalHandle
<control_msgs::action::FollowJointTrajectory>::WrappedResult & result )
{
printf( "common_result_response time: %f\n", rclcpp::Clock( ).now( ).seconds( ) );
common_resultcode = result.code;
common_action_result_code = result.result->error_code;
switch ( result.code ) {
case rclcpp_action::ResultCode::SUCCEEDED:
printf( "SUCCEEDED result code\n" );
break;
case rclcpp_action::ResultCode::ABORTED:
printf( "Goal was aborted\n" );
return;
case rclcpp_action::ResultCode::CANCELED:
printf( "Goal was canceled\n" );
return;
default:
printf( "Unknown result code\n" );
return;
}
}
68 void common_feedback(
rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr,
const std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Feedback> feedback )
{
std::cout << "feedback->desired.positions :";
for ( auto & x : feedback->desired.positions ) {
std::cout << x << "\t";
}
std::cout << std::endl;
std::cout << "feedback->desired.velocities :";
for ( auto & x : feedback->desired.velocities ) {
std::cout << x << "\t";
}
std::cout << std::endl;
}
84 int main( int argc, char * argv[] )
{
rclcpp::init( argc, argv );
node = std::make_shared<rclcpp::Node>( "trajectory_test_node" );
std::cout << "node created" << std::endl;
rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SharedPtr action_client;
action_client = rclcpp_action::create_client<control_msgs::action::FollowJointTrajectory>(
node->get_node_base_interface( ),
node->get_node_graph_interface( ),
node->get_node_logging_interface( ),
node->get_node_waitables_interface( ),
"/joint_trajectory_controller/follow_joint_trajectory" );
bool response =
action_client->wait_for_action_server( std::chrono::seconds( 1 ) );
if ( !response ) {
throw std::runtime_error( "could not get action server" );
}
std::cout << "Created action server" << std::endl;
std::vector<std::string> joint_names = {"slider_to_cart"};
std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points;
trajectory_msgs::msg::JointTrajectoryPoint point;
point.time_from_start = rclcpp::Duration::from_seconds( 0.0 ); // start asap
point.positions.resize( joint_names.size( ) );
point.positions[0] = 0.0;
trajectory_msgs::msg::JointTrajectoryPoint point2;
point2.time_from_start = rclcpp::Duration::from_seconds( 1.0 );
point2.positions.resize( joint_names.size( ) );
point2.positions[0] = -1.0;
trajectory_msgs::msg::JointTrajectoryPoint point3;
point3.time_from_start = rclcpp::Duration::from_seconds( 2.0 );
point3.positions.resize( joint_names.size( ) );
point3.positions[0] = 1.0;
trajectory_msgs::msg::JointTrajectoryPoint point4;
point4.time_from_start = rclcpp::Duration::from_seconds( 3.0 );
point4.positions.resize( joint_names.size( ) );
point4.positions[0] = 0.0;
points.push_back( point );
points.push_back( point2 );
points.push_back( point3 );
points.push_back( point4 );
rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SendGoalOptions opt;
opt.goal_response_callback = std::bind( common_goal_response, std::placeholders::_1 );
opt.result_callback = std::bind( common_result_response, std::placeholders::_1 );
opt.feedback_callback = std::bind( common_feedback, std::placeholders::_1, std::placeholders::_2 );
control_msgs::action::FollowJointTrajectory_Goal goal_msg;
goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds( 1.0 );
goal_msg.trajectory.joint_names = joint_names;
goal_msg.trajectory.points = points;
auto goal_handle_future = action_client->async_send_goal( goal_msg, opt );
if ( rclcpp::spin_until_future_complete( node, goal_handle_future ) !=
rclcpp::FutureReturnCode::SUCCESS )
{
RCLCPP_ERROR( node->get_logger( ), "send goal call failed :( " );
return 1;
}
RCLCPP_INFO( node->get_logger( ), "send goal call ok : )" );
rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr
goal_handle = goal_handle_future.get( );
if ( !goal_handle ) {
RCLCPP_ERROR( node->get_logger( ), "Goal was rejected by server" );
return 1;
}
RCLCPP_INFO( node->get_logger( ), "Goal was accepted by server" );
// Wait for the server to be done with the goal
auto result_future = action_client->async_get_result( goal_handle );
RCLCPP_INFO( node->get_logger( ), "Waiting for result" );
if ( rclcpp::spin_until_future_complete( node, result_future ) !=
rclcpp::FutureReturnCode::SUCCESS )
{
RCLCPP_ERROR( node->get_logger( ), "get result call failed :( " );
return 1;
}
std::cout << "async_send_goal" << std::endl;
rclcpp::shutdown( );
return 0;
}
1 // Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include <string>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float64_multi_array.hpp"
std::shared_ptr<rclcpp::Node> node;
25 int main( int argc, char * argv[] )
{
rclcpp::init( argc, argv );
node = std::make_shared<rclcpp::Node>( "velocity_test_node" );
auto publisher = node->create_publisher<std_msgs::msg::Float64MultiArray>(
"/velocity_controller/commands", 10 );
RCLCPP_INFO( node->get_logger( ), "node created" );
std_msgs::msg::Float64MultiArray commands;
using namespace std::chrono_literals;
commands.data.push_back( 0 );
publisher->publish( commands );
std::this_thread::sleep_for( 1s );
commands.data[0] = 1;
publisher->publish( commands );
std::this_thread::sleep_for( 1s );
commands.data[0] = -1;
publisher->publish( commands );
std::this_thread::sleep_for( 1s );
commands.data[0] = 0;
publisher->publish( commands );
std::this_thread::sleep_for( 1s );
rclcpp::shutdown( );
return 0;
}
/*
* Copyright ( c ) 2008, Willow Garage, Inc.
* All rights reserved.
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
#ifndef NAV2_AMCL__AMCL_NODE_HPP_
#define NAV2_AMCL__AMCL_NODE_HPP_
#include <atomic>
#include <map>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "message_filters/subscriber.h"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_amcl/motion_model/motion_model.hpp"
#include "nav2_amcl/sensors/laser/laser.hpp"
#include "nav2_msgs/msg/particle.hpp"
#include "nav2_msgs/msg/particle_cloud.hpp"
#include "nav_msgs/srv/set_map.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "std_srvs/srv/empty.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/transform_listener.h"
#include "pluginlib/class_loader.hpp"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#pragma GCC diagnostic ignored "-Wreorder"
#include "tf2_ros/message_filter.h"
#pragma GCC diagnostic pop
#define NEW_UNIFORM_SAMPLING 1
namespace nav2_amcl
{
/*
* @class AmclNode
* @brief ROS wrapper for AMCL
*/
59 class AmclNode : public nav2_util::LifecycleNode
{
public:
/*
* @brief AMCL constructor
* @param options Additional options to control creation of the node.
*/
66 explicit AmclNode( const rclcpp::NodeOptions & options = rclcpp::NodeOptions( ) );
/*
* @brief AMCL destructor
*/
70 ~AmclNode( );
protected:
/*
* @brief Lifecycle configure
*/
nav2_util::CallbackReturn on_configure( const rclcpp_lifecycle::State & state ) override;
/*
* @brief Lifecycle activate
*/
nav2_util::CallbackReturn on_activate( const rclcpp_lifecycle::State & state ) override;
/*
* @brief Lifecycle deactivate
*/
nav2_util::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & state ) override;
/*
* @brief Lifecycle cleanup
*/
nav2_util::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & state ) override;
/*
* @brief Lifecycle shutdown
*/
nav2_util::CallbackReturn on_shutdown( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Callback executed when a parameter change is detected
* @param event ParameterEvent message
*/
rcl_interfaces::msg::SetParametersResult
dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters );
// Dynamic parameters handler
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
// Since the sensor data from gazebo or the robot is not lifecycle enabled, we won't
// respond until we're in the active state
std::atomic<bool> active_{false};
// Dedicated callback group and executor for services and subscriptions in AmclNode,
// in order to isolate TF timer used in message filter.
rclcpp::CallbackGroup::SharedPtr callback_group_;
rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
std::unique_ptr<nav2_util::NodeThread> executor_thread_;
// Pose hypothesis
typedef struct
{
double weight; // Total weight ( weights sum to 1 )
pf_vector_t pf_pose_mean; // Mean of pose esimate
pf_matrix_t pf_pose_cov; // Covariance of pose estimate
} amcl_hyp_t;
// Map-related
/*
* @brief Get new map from ROS topic to localize in
* @param msg Map message
*/
127 void mapReceived( const nav_msgs::msg::OccupancyGrid::SharedPtr msg );
/*
* @brief Handle a new map message
* @param msg Map message
*/
132 void handleMapMessage( const nav_msgs::msg::OccupancyGrid & msg );
/*
* @brief Creates lookup table of free cells in map
*/
136 void createFreeSpaceVector( );
/*
* @brief Frees allocated map related memory
*/
140 void freeMapDependentMemory( );
141 map_t * map_{nullptr};
/*
* @brief Convert an occupancy grid map to an AMCL map
* @param map_msg Map message
* @return pointer to map for AMCL to use
*/
147 map_t * convertMap( const nav_msgs::msg::OccupancyGrid & map_msg );
bool first_map_only_{true};
std::atomic<bool> first_map_received_{false};
amcl_hyp_t * initial_pose_hyp_;
std::recursive_mutex mutex_;
rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::ConstSharedPtr map_sub_;
#if NEW_UNIFORM_SAMPLING
static std::vector<std::pair<int, int>> free_space_indices;
#endif
// Transforms
/*
* @brief Initialize required ROS transformations
*/
void initTransforms( );
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
bool sent_first_transform_{false};
bool latest_tf_valid_{false};
tf2::Transform latest_tf_;
// Message filters
/*
* @brief Initialize incoming data message subscribers and filters
*/
void initMessageFilters( );
std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan,
rclcpp_lifecycle::LifecycleNode>> laser_scan_sub_;
std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>> laser_scan_filter_;
message_filters::Connection laser_scan_connection_;
// Publishers and subscribers
/*
* @brief Initialize pub subs of AMCL
*/
void initPubSub( );
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::ConstSharedPtr
initial_pose_sub_;
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
pose_pub_;
rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::ParticleCloud>::SharedPtr
particle_cloud_pub_;
/*
* @brief Handle with an initial pose estimate is received
*/
void initialPoseReceived( geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg );
/*
* @brief Handle when a laser scan is received
*/
void laserReceived( sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan );
// Services and service callbacks
/*
* @brief Initialize state services
*/
void initServices( );
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr global_loc_srv_;
/*
* @brief Service callback for a global relocalization request
*/
void globalLocalizationCallback(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<std_srvs::srv::Empty::Request> request,
std::shared_ptr<std_srvs::srv::Empty::Response> response );
// Let amcl update samples without requiring motion
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr nomotion_update_srv_;
/*
* @brief Request an AMCL update even though the robot hasn't moved
*/
void nomotionUpdateCallback(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<std_srvs::srv::Empty::Request> request,
std::shared_ptr<std_srvs::srv::Empty::Response> response );
// Nomotion update control. Used to temporarily let amcl update samples even when no motion occurs
std::atomic<bool> force_update_{false};
// Odometry
/*
* @brief Initialize odometry
*/
void initOdometry( );
std::shared_ptr<nav2_amcl::MotionModel> motion_model_;
geometry_msgs::msg::PoseStamped latest_odom_pose_;
geometry_msgs::msg::PoseWithCovarianceStamped last_published_pose_;
double init_pose_[3]; // Initial robot pose
double init_cov_[3];
pluginlib::ClassLoader<nav2_amcl::MotionModel> plugin_loader_{"nav2_amcl",
"nav2_amcl::MotionModel"};
/*
* @brief Get robot pose in odom frame using TF
*/
bool getOdomPose(
// Helper to get odometric pose from transform system
geometry_msgs::msg::PoseStamped & pose,
double & x, double & y, double & yaw,
const rclcpp::Time & sensor_timestamp, const std::string & frame_id );
std::atomic<bool> first_pose_sent_;
// Particle filter
/*
* @brief Initialize particle filter
*/
void initParticleFilter( );
/*
* @brief Pose-generating function used to uniformly distribute particles over the map
*/
static pf_vector_t uniformPoseGenerator( void * arg );
pf_t * pf_{nullptr};
bool pf_init_;
pf_vector_t pf_odom_pose_;
int resample_count_{0};
// Laser scan related
/*
* @brief Initialize laser scan
*/
void initLaserScan( );
/*
* @brief Create a laser object
*/
nav2_amcl::Laser * createLaserObject( );
int scan_error_count_{0};
std::vector<nav2_amcl::Laser *> lasers_;
std::vector<bool> lasers_update_;
std::map<std::string, int> frame_to_laser_;
rclcpp::Time last_laser_received_ts_;
/*
* @brief Check if sufficient time has elapsed to get an update
*/
bool checkElapsedTime( std::chrono::seconds check_interval, rclcpp::Time last_time );
rclcpp::Time last_time_printed_msg_;
/*
* @brief Add a new laser scanner if a new one is received in the laser scallbacks
*/
bool addNewScanner(
int & laser_index,
const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
const std::string & laser_scan_frame_id,
geometry_msgs::msg::PoseStamped & laser_pose );
/*
* @brief Whether the pf needs to be updated
*/
bool shouldUpdateFilter( const pf_vector_t pose, pf_vector_t & delta );
/*
* @brief Update the PF
*/
bool updateFilter(
const int & laser_index,
const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
const pf_vector_t & pose );
/*
* @brief Publish particle cloud
*/
void publishParticleCloud( const pf_sample_set_t * set );
/*
* @brief Get the current state estimat hypothesis from the particle cloud
*/
bool getMaxWeightHyp(
std::vector<amcl_hyp_t> & hyps, amcl_hyp_t & max_weight_hyps,
int & max_weight_hyp );
/*
* @brief Publish robot pose in map frame from AMCL
*/
void publishAmclPose(
const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
const std::vector<amcl_hyp_t> & hyps, const int & max_weight_hyp );
/*
* @brief Determine TF transformation from map to odom
*/
void calculateMaptoOdomTransform(
const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
const std::vector<amcl_hyp_t> & hyps,
const int & max_weight_hyp );
/*
* @brief Publish TF transformation from map to odom
*/
void sendMapToOdomTransform( const tf2::TimePoint & transform_expiration );
/*
* @brief Handle a new pose estimate callback
*/
void handleInitialPose( geometry_msgs::msg::PoseWithCovarianceStamped & msg );
bool init_pose_received_on_inactive{false};
bool initial_pose_is_known_{false};
bool set_initial_pose_{false};
bool always_reset_initial_pose_;
double initial_pose_x_;
double initial_pose_y_;
double initial_pose_z_;
double initial_pose_yaw_;
/*
* @brief Get ROS parameters for node
*/
void initParameters( );
double alpha1_;
double alpha2_;
double alpha3_;
double alpha4_;
double alpha5_;
std::string base_frame_id_;
double beam_skip_distance_;
double beam_skip_error_threshold_;
double beam_skip_threshold_;
bool do_beamskip_;
std::string global_frame_id_;
double lambda_short_;
double laser_likelihood_max_dist_;
double laser_max_range_;
double laser_min_range_;
std::string sensor_model_type_;
int max_beams_;
int max_particles_;
int min_particles_;
std::string odom_frame_id_;
double pf_err_;
double pf_z_;
double alpha_fast_;
double alpha_slow_;
int resample_interval_;
std::string robot_model_type_;
tf2::Duration save_pose_period_;
double sigma_hit_;
bool tf_broadcast_;
tf2::Duration transform_tolerance_;
double a_thresh_;
double d_thresh_;
double z_hit_;
double z_max_;
double z_short_;
double z_rand_;
std::string scan_topic_{"scan"};
std::string map_topic_{"map"};
};
} // namespace nav2_amcl
#endif // NAV2_AMCL__AMCL_NODE_HPP_
1 /*
* Player - One Hell of a Robot Server
* Copyright ( C ) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
#ifndef NAV2_AMCL__ANGLEUTILS_HPP_
#define NAV2_AMCL__ANGLEUTILS_HPP_
#include <math.h>
namespace nav2_amcl
{
/*
* @class angleutils
* @brief Some utilities for working with angles
*/
34 class angleutils
{
public:
/*
* @brief Normalize angles
* @brief z Angle to normalize
* @return normalized angle
*/
42 static double normalize( double z );
/*
* @brief Find minimum distance between 2 angles
* @brief a Angle 1
* @brief b Angle 2
* @return normalized angle difference
*/
50 static double angle_diff( double a, double b );
};
inline double
54 angleutils::normalize( double z )
{
return atan2( sin( z ), cos( z ) );
}
inline double
60 angleutils::angle_diff( double a, double b )
{
a = normalize( a );
b = normalize( b );
double d1 = a - b;
double d2 = 2 * M_PI - fabs( d1 );
if ( d1 > 0 ) {
d2 *= -1.0;
}
if ( fabs( d1 ) < fabs( d2 ) ) {
return d1;
} else {
return d2;
}
}
} // namespace nav2_amcl
#endif // NAV2_AMCL__ANGLEUTILS_HPP_
1 /*
* Player - One Hell of a Robot Server
* Copyright ( C ) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Global map ( grid-based )
* Author: Andrew Howard
* Date: 6 Feb 2003
* CVS: $Id: map.h 1713 2003-08-23 04:03:43Z inspectorg $
**************************************************************************/
#ifndef NAV2_AMCL__MAP__MAP_HPP_
#define NAV2_AMCL__MAP__MAP_HPP_
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
// Forward declarations
struct _rtk_fig_t;
// Limits
#define MAP_WIFI_MAX_LEVELS 8
// Description for a single map cell.
typedef struct
{
// Occupancy state ( -1 = free, 0 = unknown, +1 = occ )
int occ_state;
// Distance to the nearest occupied cell
double occ_dist;
// Wifi levels
// int wifi_levels[MAP_WIFI_MAX_LEVELS];
} map_cell_t;
// Description for a map
typedef struct
{
// Map origin; the map is a viewport onto a conceptual larger map.
double origin_x, origin_y;
// Map scale ( m/cell )
double scale;
// Map dimensions ( number of cells )
int size_x, size_y;
// The map data, stored as a grid
map_cell_t * cells;
// Max distance at which we care about obstacles, for constructing
// likelihood field
double max_occ_dist;
} map_t;
/**************************************************************************
* Basic map functions
**************************************************************************/
// Create a new ( empty ) map
85 map_t * map_alloc( void );
// Destroy a map
88 void map_free( map_t * map );
// Update the cspace distances
91 void map_update_cspace( map_t * map, double max_occ_dist );
/**************************************************************************
* Range functions
**************************************************************************/
// Extract a single range reading from the map
99 double map_calc_range( map_t * map, double ox, double oy, double oa, double max_range );
/**************************************************************************
* GUI/diagnostic functions
**************************************************************************/
// Draw the occupancy grid
107 void map_draw_occ( map_t * map, struct _rtk_fig_t * fig );
// Draw the cspace map
110 void map_draw_cspace( map_t * map, struct _rtk_fig_t * fig );
// Draw a wifi map
113 void map_draw_wifi( map_t * map, struct _rtk_fig_t * fig, int index );
/**************************************************************************
* Map manipulation macros
**************************************************************************/
// Convert from map index to world coords
#define MAP_WXGX( map, i ) ( map->origin_x + ( ( i ) - map->size_x / 2 ) * map->scale )
#define MAP_WYGY( map, j ) ( map->origin_y + ( ( j ) - map->size_y / 2 ) * map->scale )
// Convert from world coords to map coords
#define MAP_GXWX( map, x ) ( floor( ( x - map->origin_x ) / map->scale + 0.5 ) + map->size_x / 2 )
#define MAP_GYWY( map, y ) ( floor( ( y - map->origin_y ) / map->scale + 0.5 ) + map->size_y / 2 )
// Test to see if the given map coords lie within the absolute map bounds.
#define MAP_VALID( map, i, j ) ( ( i >= 0 ) && ( i < map->size_x ) && ( j >= 0 ) && ( j < map->size_y ) )
// Compute the cell index for the given map coords.
#define MAP_INDEX( map, i, j ) ( ( i ) + ( j ) * map->size_x )
#ifdef __cplusplus
}
#endif
#endif // NAV2_AMCL__MAP__MAP_HPP_
1 /*
* Player - One Hell of a Robot Server
* Copyright ( C ) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
#ifndef NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_
#define NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_
#include <sys/types.h>
#include <math.h>
#include <algorithm>
#include "nav2_amcl/motion_model/motion_model.hpp"
#include "nav2_amcl/angleutils.hpp"
namespace nav2_amcl
{
35 class DifferentialMotionModel : public nav2_amcl::MotionModel
{
public:
38 virtual void initialize(
double alpha1, double alpha2, double alpha3, double alpha4,
double alpha5 );
41 virtual void odometryUpdate( pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta );
private:
double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_;
};
} // namespace nav2_amcl
#endif // NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or ( at your option ) any later version.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
#ifndef NAV2_AMCL__MOTION_MODEL__MOTION_MODEL_HPP_
#define NAV2_AMCL__MOTION_MODEL__MOTION_MODEL_HPP_
#include <string>
#include <memory>
#include "nav2_amcl/pf/pf.hpp"
#include "nav2_amcl/pf/pf_pdf.hpp"
namespace nav2_amcl
{
/**
* @class nav2_amcl::MotionModel
* @brief An abstract motion model class
*/
33 class MotionModel
{
public:
virtual ~MotionModel( ) = default;
/**
* @brief An factory to create motion models
* @param type Type of motion model to create in factory
* @param alpha1 error parameters, see documentation
* @param alpha2 error parameters, see documentation
* @param alpha3 error parameters, see documentation
* @param alpha4 error parameters, see documentation
* @param alpha5 error parameters, see documentation
* @return MotionModel A pointer to the motion model it created
*/
virtual void initialize(
double alpha1, double alpha2, double alpha3, double alpha4,
double alpha5 ) = 0;
/**
* @brief Update on new odometry data
* @param pf The particle filter to update
* @param pose pose of robot in odometry update
* @param delta change in pose in odometry update
*/
virtual void odometryUpdate( pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta ) = 0;
};
} // namespace nav2_amcl
#endif // NAV2_AMCL__MOTION_MODEL__MOTION_MODEL_HPP_
1 /*
* Player - One Hell of a Robot Server
* Copyright ( C ) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
#ifndef NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_
#define NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_
#include <sys/types.h>
#include <math.h>
#include <algorithm>
#include "nav2_amcl/motion_model/motion_model.hpp"
#include "nav2_amcl/angleutils.hpp"
namespace nav2_amcl
{
35 class OmniMotionModel : public nav2_amcl::MotionModel
{
public:
38 virtual void initialize(
double alpha1, double alpha2, double alpha3, double alpha4,
double alpha5 );
41 virtual void odometryUpdate( pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta );
private:
double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_;
};
} // namespace nav2_amcl
#endif // NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_
1 /*
* Player - One Hell of a Robot Server
* Copyright ( C ) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/* Eigen-decomposition for symmetric 3x3 real matrices.
Public domain, copied from the public domain Java library JAMA. */
#ifndef NAV2_AMCL__PF__EIG3_HPP_
#define NAV2_AMCL__PF__EIG3_HPP_
/* Symmetric matrix A => eigenvectors in columns of V, corresponding
eigenvalues in d. */
29 void eigen_decomposition( double A[3][3], double V[3][3], double d[3] );
#endif // NAV2_AMCL__PF__EIG3_HPP_
1 /*
* Player - One Hell of a Robot Server
* Copyright ( C ) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Simple particle filter for localization.
* Author: Andrew Howard
* Date: 10 Dec 2002
* CVS: $Id: pf.h 3293 2005-11-19 08:37:45Z gerkey $
*************************************************************************/
#ifndef NAV2_AMCL__PF__PF_HPP_
#define NAV2_AMCL__PF__PF_HPP_
#include "nav2_amcl/pf/pf_vector.hpp"
#include "nav2_amcl/pf/pf_kdtree.hpp"
#ifdef __cplusplus
extern "C" {
#endif
// Forward declarations
struct _pf_t;
struct _rtk_fig_t;
struct _pf_sample_set_t;
// Function prototype for the initialization model; generates a sample pose from
// an appropriate distribution.
typedef pf_vector_t ( * pf_init_model_fn_t ) ( void * init_data );
// Function prototype for the action model; generates a sample pose from
// an appropriate distribution
typedef void ( * pf_action_model_fn_t ) (
void * action_data,
struct _pf_sample_set_t * set );
// Function prototype for the sensor model; determines the probability
// for the given set of sample poses.
typedef double ( * pf_sensor_model_fn_t ) (
void * sensor_data,
struct _pf_sample_set_t * set );
// Information for a single sample
typedef struct
{
// Pose represented by this sample
pf_vector_t pose;
// Weight for this pose
double weight;
} pf_sample_t;
// Information for a cluster of samples
typedef struct
{
// Number of samples
int count;
// Total weight of samples in this cluster
double weight;
// Cluster statistics
pf_vector_t mean;
pf_matrix_t cov;
// Workspace
double m[4], c[2][2];
} pf_cluster_t;
// Information for a set of samples
typedef struct _pf_sample_set_t
{
// The samples
int sample_count;
pf_sample_t * samples;
// A kdtree encoding the histogram
pf_kdtree_t * kdtree;
// Clusters
int cluster_count, cluster_max_count;
pf_cluster_t * clusters;
// Filter statistics
pf_vector_t mean;
pf_matrix_t cov;
int converged;
} pf_sample_set_t;
// Information for an entire filter
typedef struct _pf_t
{
// This min and max number of samples
int min_samples, max_samples;
// Population size parameters
double pop_err, pop_z;
// The sample sets. We keep two sets and use [current_set]
// to identify the active set.
int current_set;
pf_sample_set_t sets[2];
// Running averages, slow and fast, of likelihood
double w_slow, w_fast;
// Decay rates for running averages
double alpha_slow, alpha_fast;
// Function used to draw random pose samples
pf_init_model_fn_t random_pose_fn;
void * random_pose_data;
double dist_threshold; // distance threshold in each axis over which the pf is considered to not
// be converged
int converged;
} pf_t;
// Create a new filter
141 pf_t * pf_alloc(
int min_samples, int max_samples,
double alpha_slow, double alpha_fast,
pf_init_model_fn_t random_pose_fn, void * random_pose_data );
// Free an existing filter
147 void pf_free( pf_t * pf );
// Initialize the filter using a guassian
150 void pf_init( pf_t * pf, pf_vector_t mean, pf_matrix_t cov );
// Initialize the filter using some model
153 void pf_init_model( pf_t * pf, pf_init_model_fn_t init_fn, void * init_data );
// Update the filter with some new action
// void pf_update_action( pf_t * pf, pf_action_model_fn_t action_fn, void * action_data );
// Update the filter with some new sensor observation
159 void pf_update_sensor( pf_t * pf, pf_sensor_model_fn_t sensor_fn, void * sensor_data );
// Resample the distribution
162 void pf_update_resample( pf_t * pf );
// Compute the CEP statistics ( mean and variance ).
// void pf_get_cep_stats( pf_t * pf, pf_vector_t * mean, double * var );
// Compute the statistics for a particular cluster. Returns 0 if
// there is no such cluster.
169 int pf_get_cluster_stats(
pf_t * pf, int cluster, double * weight,
pf_vector_t * mean, pf_matrix_t * cov );
// Re-compute the cluster statistics for a sample set
174 void pf_cluster_stats( pf_t * pf, pf_sample_set_t * set );
// Display the sample set
178 void pf_draw_samples( pf_t * pf, struct _rtk_fig_t * fig, int max_samples );
// Draw the histogram ( kdtree )
181 void pf_draw_hist( pf_t * pf, struct _rtk_fig_t * fig );
// Draw the CEP statistics
// void pf_draw_cep_stats( pf_t * pf, struct _rtk_fig_t * fig );
// Draw the cluster statistics
187 void pf_draw_cluster_stats( pf_t * pf, struct _rtk_fig_t * fig );
// calculate if the particle filter has converged -
// and sets the converged flag in the current set and the pf
191 int pf_update_converged( pf_t * pf );
// sets the current set and pf converged values to zero
194 void pf_init_converged( pf_t * pf );
#ifdef __cplusplus
}
#endif
#endif // NAV2_AMCL__PF__PF_HPP_
1 /*
* Player - One Hell of a Robot Server
* Copyright ( C ) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: KD tree functions
* Author: Andrew Howard
* Date: 18 Dec 2002
* CVS: $Id: pf_kdtree.h 6532 2008-06-11 02:45:56Z gbiggs $
*************************************************************************/
#ifndef NAV2_AMCL__PF__PF_KDTREE_HPP_
#define NAV2_AMCL__PF__PF_KDTREE_HPP_
#ifdef INCLUDE_RTKGUI
#include <rtk.h>
#endif
// Info for a node in the tree
typedef struct pf_kdtree_node
{
// Depth in the tree
int leaf, depth;
// Pivot dimension and value
int pivot_dim;
double pivot_value;
// The key for this node
int key[3];
// The value for this node
double value;
// The cluster label ( leaf nodes )
int cluster;
// Child nodes
struct pf_kdtree_node * children[2];
} pf_kdtree_node_t;
// A kd tree
typedef struct
{
// Cell size
double size[3];
// The root node of the tree
pf_kdtree_node_t * root;
// The number of nodes in the tree
int node_count, node_max_count;
pf_kdtree_node_t * nodes;
// The number of leaf nodes in the tree
int leaf_count;
} pf_kdtree_t;
// Create a tree
79 extern pf_kdtree_t * pf_kdtree_alloc( int max_size );
// Destroy a tree
82 extern void pf_kdtree_free( pf_kdtree_t * self );
// Clear all entries from the tree
85 extern void pf_kdtree_clear( pf_kdtree_t * self );
// Insert a pose into the tree
88 extern void pf_kdtree_insert( pf_kdtree_t * self, pf_vector_t pose, double value );
// Cluster the leaves in the tree
91 extern void pf_kdtree_cluster( pf_kdtree_t * self );
// Determine the probability estimate for the given pose
// extern double pf_kdtree_get_prob( pf_kdtree_t * self, pf_vector_t pose );
// Determine the cluster label for the given pose
97 extern int pf_kdtree_get_cluster( pf_kdtree_t * self, pf_vector_t pose );
#ifdef INCLUDE_RTKGUI
// Draw the tree
103 extern void pf_kdtree_draw( pf_kdtree_t * self, rtk_fig_t * fig );
#endif
#endif // NAV2_AMCL__PF__PF_KDTREE_HPP_
1 /*
* Player - One Hell of a Robot Server
* Copyright ( C ) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Useful pdf functions
* Author: Andrew Howard
* Date: 10 Dec 2002
* CVS: $Id: pf_pdf.h 6345 2008-04-17 01:36:39Z gerkey $
*************************************************************************/
#ifndef NAV2_AMCL__PF__PF_PDF_HPP_
#define NAV2_AMCL__PF__PF_PDF_HPP_
#include "nav2_amcl/pf/pf_vector.hpp"
// #include <gsl/gsl_rng.h>
// #include <gsl/gsl_randist.h>
#ifdef __cplusplus
extern "C" {
#endif
/**************************************************************************
* Gaussian
*************************************************************************/
// Gaussian PDF info
typedef struct
{
// Mean, covariance and inverse covariance
pf_vector_t x;
pf_matrix_t cx;
// pf_matrix_t cxi;
double cxdet;
// Decomposed covariance matrix ( rotation * diagonal )
pf_matrix_t cr;
pf_vector_t cd;
// A random number generator
// gsl_rng *rng;
} pf_pdf_gaussian_t;
// Create a gaussian pdf
63 pf_pdf_gaussian_t * pf_pdf_gaussian_alloc( pf_vector_t x, pf_matrix_t cx );
// Destroy the pdf
66 void pf_pdf_gaussian_free( pf_pdf_gaussian_t * pdf );
// Compute the value of the pdf at some point [z].
// double pf_pdf_gaussian_value( pf_pdf_gaussian_t *pdf, pf_vector_t z );
// Draw randomly from a zero-mean Gaussian distribution, with standard
// deviation sigma.
// We use the polar form of the Box-Muller transformation, explained here:
// http://www.taygeta.com/random/gaussian.html
75 double pf_ran_gaussian( double sigma );
// Generate a sample from the pdf.
78 pf_vector_t pf_pdf_gaussian_sample( pf_pdf_gaussian_t * pdf );
#ifdef __cplusplus
}
#endif
#endif // NAV2_AMCL__PF__PF_PDF_HPP_
1 /*
* Player - One Hell of a Robot Server
* Copyright ( C ) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Vector functions
* Author: Andrew Howard
* Date: 10 Dec 2002
* CVS: $Id: pf_vector.h 6345 2008-04-17 01:36:39Z gerkey $
*************************************************************************/
#ifndef NAV2_AMCL__PF__PF_VECTOR_HPP_
#define NAV2_AMCL__PF__PF_VECTOR_HPP_
#ifdef __cplusplus
extern "C" {
#endif
#include <stdio.h>
// The basic vector
typedef struct
{
double v[3];
} pf_vector_t;
// The basic matrix
typedef struct
{
double m[3][3];
} pf_matrix_t;
// Return a zero vector
52 pf_vector_t pf_vector_zero( );
// Check for NAN or INF in any component
// int pf_vector_finite( pf_vector_t a );
// Print a vector
// void pf_vector_fprintf( pf_vector_t s, FILE * file, const char * fmt );
// Simple vector addition
// pf_vector_t pf_vector_add( pf_vector_t a, pf_vector_t b );
// Simple vector subtraction
64 pf_vector_t pf_vector_sub( pf_vector_t a, pf_vector_t b );
// Transform from local to global coords ( a + b )
67 pf_vector_t pf_vector_coord_add( pf_vector_t a, pf_vector_t b );
// Transform from global to local coords ( a - b )
// pf_vector_t pf_vector_coord_sub( pf_vector_t a, pf_vector_t b );
// Return a zero matrix
74 pf_matrix_t pf_matrix_zero( );
// Check for NAN or INF in any component
// int pf_matrix_finite( pf_matrix_t a );
// Print a matrix
// void pf_matrix_fprintf( pf_matrix_t s, FILE * file, const char * fmt );
// Compute the matrix inverse. Will also return the determinant,
// which should be checked for underflow ( indicated singular matrix ).
// pf_matrix_t pf_matrix_inverse( pf_matrix_t a, double *det );
// Decompose a covariance matrix [a] into a rotation matrix [r] and a
// diagonal matrix [d] such that a = r * d * r^T.
88 void pf_matrix_unitary( pf_matrix_t * r, pf_matrix_t * d, pf_matrix_t a );
#ifdef __cplusplus
}
#endif
#endif // NAV2_AMCL__PF__PF_VECTOR_HPP_
1 // Copyright ( c ) Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_AMCL__PORTABLE_UTILS_HPP_
#define NAV2_AMCL__PORTABLE_UTILS_HPP_
#include <stdlib.h>
#ifdef __cplusplus
extern "C" {
#endif
#ifndef HAVE_DRAND48
// Some system ( e.g., Windows ) doesn't come with drand48( ), srand48( ).
// Use rand, and srand for such system.
26 static double drand48( void )
{
return ( ( double )rand( ) ) / RAND_MAX;// NOLINT
}
31 static void srand48( long int seedval )// NOLINT
{
srand( seedval );
}
#endif
#ifdef __cplusplus
}
#endif
#endif // NAV2_AMCL__PORTABLE_UTILS_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or ( at your option ) any later version.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
#ifndef NAV2_AMCL__SENSORS__LASER__LASER_HPP_
#define NAV2_AMCL__SENSORS__LASER__LASER_HPP_
#include <string>
#include "nav2_amcl/pf/pf.hpp"
#include "nav2_amcl/pf/pf_pdf.hpp"
#include "nav2_amcl/map/map.hpp"
namespace nav2_amcl
{
// Forward declarations
30 class LaserData;
/*
* @class Laser
* @brief Base class for laser sensor models
*/
36 class Laser
{
public:
/**
* @brief A Laser constructor
* @param max_beams number of beams to use
* @param map Map pointer to use
*/
44 Laser( size_t max_beams, map_t * map );
/*
* @brief Laser destructor
*/
49 virtual ~Laser( );
/*
* @brief Run a sensor update on laser
* @param pf Particle filter to use
* @param data Laser data to use
* @return if it was succesful
*/
57 virtual bool sensorUpdate( pf_t * pf, LaserData * data ) = 0;
/*
* @brief Set the laser pose from an update
* @param laser_pose Pose of the laser
*/
63 void SetLaserPose( pf_vector_t & laser_pose );
protected:
double z_hit_;
double z_rand_;
double sigma_hit_;
/*
* @brief Reallocate weights
* @param max_samples Max number of samples
* @param max_obs number of observations
*/
75 void reallocTempData( int max_samples, int max_obs );
76 map_t * map_;
77 pf_vector_t laser_pose_;
int max_beams_;
int max_samples_;
int max_obs_;
double ** temp_obs_;
};
/*
* @class LaserData
* @brief Class of laser data to process
*/
88 class LaserData
{
public:
91 Laser * laser;
/*
* @brief LaserData constructor
*/
96 LaserData( ) {ranges = NULL;}
/*
* @brief LaserData destructor
*/
100 virtual ~LaserData( ) {delete[] ranges;}
public:
int range_count;
double range_max;
double( *ranges )[2];
};
/*
* @class BeamModel
* @brief Beam model laser sensor
*/
112 class BeamModel : public Laser
{
public:
/*
* @brief BeamModel constructor
*/
118 BeamModel(
double z_hit, double z_short, double z_max, double z_rand, double sigma_hit,
120 double lambda_short, double chi_outlier, size_t max_beams, map_t * map );
/*
* @brief Run a sensor update on laser
* @param pf Particle filter to use
* @param data Laser data to use
* @return if it was succesful
*/
128 bool sensorUpdate( pf_t * pf, LaserData * data );
private:
131 static double sensorFunction( LaserData * data, pf_sample_set_t * set );
double z_short_;
double z_max_;
double lambda_short_;
double chi_outlier_;
};
/*
* @class LikelihoodFieldModel
* @brief likelihood field model laser sensor
*/
142 class LikelihoodFieldModel : public Laser
{
public:
/*
* @brief BeamModel constructor
*/
148 LikelihoodFieldModel(
double z_hit, double z_rand, double sigma_hit, double max_occ_dist,
150 size_t max_beams, map_t * map );
/*
* @brief Run a sensor update on laser
* @param pf Particle filter to use
* @param data Laser data to use
* @return if it was succesful
*/
158 bool sensorUpdate( pf_t * pf, LaserData * data );
private:
/*
* @brief Perform the update function
* @param data Laser data to use
* @param pf Particle filter to use
* @return if it was succesful
*/
167 static double sensorFunction( LaserData * data, pf_sample_set_t * set );
};
/*
* @class LikelihoodFieldModelProb
* @brief likelihood prob model laser sensor
*/
174 class LikelihoodFieldModelProb : public Laser
{
public:
/*
* @brief BeamModel constructor
*/
180 LikelihoodFieldModelProb(
double z_hit, double z_rand, double sigma_hit, double max_occ_dist,
182 bool do_beamskip, double beam_skip_distance,
double beam_skip_threshold, double beam_skip_error_threshold,
184 size_t max_beams, map_t * map );
/*
* @brief Run a sensor update on laser
* @param pf Particle filter to use
* @param data Laser data to use
* @return if it was succesful
*/
192 bool sensorUpdate( pf_t * pf, LaserData * data );
private:
/*
* @brief Perform the update function
* @param data Laser data to use
* @param pf Particle filter to use
* @return if it was succesful
*/
201 static double sensorFunction( LaserData * data, pf_sample_set_t * set );
202 bool do_beamskip_;
double beam_skip_distance_;
double beam_skip_threshold_;
double beam_skip_error_threshold_;
};
} // namespace nav2_amcl
#endif // NAV2_AMCL__SENSORS__LASER__LASER_HPP_
/*
* Copyright ( c ) 2008, Willow Garage, Inc.
* All rights reserved.
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/* Author: Brian Gerkey */
#include "nav2_amcl/amcl_node.hpp"
#include <algorithm>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "message_filters/subscriber.h"
#include "nav2_amcl/angleutils.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_amcl/pf/pf.hpp"
#include "nav2_util/string_utils.hpp"
#include "nav2_amcl/sensors/laser/laser.hpp"
#include "tf2/convert.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2/LinearMath/Transform.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/message_filter.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/create_timer_ros.h"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpedantic"
#include "tf2/utils.h"
#pragma GCC diagnostic pop
#include "nav2_amcl/portable_utils.hpp"
using namespace std::placeholders;
using rcl_interfaces::msg::ParameterType;
using namespace std::chrono_literals;
namespace nav2_amcl
{
using nav2_util::geometry_utils::orientationAroundZAxis;
61 AmclNode::AmclNode( const rclcpp::NodeOptions & options )
: nav2_util::LifecycleNode( "amcl", "", options )
{
RCLCPP_INFO( get_logger( ), "Creating" );
add_parameter(
"alpha1", rclcpp::ParameterValue( 0.2 ),
"This is the alpha1 parameter", "These are additional constraints for alpha1" );
add_parameter(
"alpha2", rclcpp::ParameterValue( 0.2 ),
"This is the alpha2 parameter", "These are additional constraints for alpha2" );
add_parameter(
"alpha3", rclcpp::ParameterValue( 0.2 ),
"This is the alpha3 parameter", "These are additional constraints for alpha3" );
add_parameter(
"alpha4", rclcpp::ParameterValue( 0.2 ),
"This is the alpha4 parameter", "These are additional constraints for alpha4" );
add_parameter(
"alpha5", rclcpp::ParameterValue( 0.2 ),
"This is the alpha5 parameter", "These are additional constraints for alpha5" );
add_parameter(
"base_frame_id", rclcpp::ParameterValue( std::string( "base_footprint" ) ),
"Which frame to use for the robot base" );
add_parameter( "beam_skip_distance", rclcpp::ParameterValue( 0.5 ) );
add_parameter( "beam_skip_error_threshold", rclcpp::ParameterValue( 0.9 ) );
add_parameter( "beam_skip_threshold", rclcpp::ParameterValue( 0.3 ) );
add_parameter( "do_beamskip", rclcpp::ParameterValue( false ) );
add_parameter(
"global_frame_id", rclcpp::ParameterValue( std::string( "map" ) ),
"The name of the coordinate frame published by the localization system" );
add_parameter(
"lambda_short", rclcpp::ParameterValue( 0.1 ),
"Exponential decay parameter for z_short part of model" );
add_parameter(
"laser_likelihood_max_dist", rclcpp::ParameterValue( 2.0 ),
"Maximum distance to do obstacle inflation on map, for use in likelihood_field model" );
add_parameter(
"laser_max_range", rclcpp::ParameterValue( 100.0 ),
"Maximum scan range to be considered",
"-1.0 will cause the laser's reported maximum range to be used" );
add_parameter(
"laser_min_range", rclcpp::ParameterValue( -1.0 ),
"Minimum scan range to be considered",
"-1.0 will cause the laser's reported minimum range to be used" );
add_parameter(
"laser_model_type", rclcpp::ParameterValue( std::string( "likelihood_field" ) ),
"Which model to use, either beam, likelihood_field, or likelihood_field_prob",
"Same as likelihood_field but incorporates the beamskip feature, if enabled" );
add_parameter(
"set_initial_pose", rclcpp::ParameterValue( false ),
"Causes AMCL to set initial pose from the initial_pose* parameters instead of "
"waiting for the initial_pose message" );
add_parameter(
"initial_pose.x", rclcpp::ParameterValue( 0.0 ),
"X coordinate of the initial robot pose in the map frame" );
add_parameter(
"initial_pose.y", rclcpp::ParameterValue( 0.0 ),
"Y coordinate of the initial robot pose in the map frame" );
add_parameter(
"initial_pose.z", rclcpp::ParameterValue( 0.0 ),
"Z coordinate of the initial robot pose in the map frame" );
add_parameter(
"initial_pose.yaw", rclcpp::ParameterValue( 0.0 ),
"Yaw of the initial robot pose in the map frame" );
add_parameter(
"max_beams", rclcpp::ParameterValue( 60 ),
"How many evenly-spaced beams in each scan to be used when updating the filter" );
add_parameter(
"max_particles", rclcpp::ParameterValue( 2000 ),
"Minimum allowed number of particles" );
add_parameter(
"min_particles", rclcpp::ParameterValue( 500 ),
"Maximum allowed number of particles" );
add_parameter(
"odom_frame_id", rclcpp::ParameterValue( std::string( "odom" ) ),
"Which frame to use for odometry" );
add_parameter( "pf_err", rclcpp::ParameterValue( 0.05 ) );
add_parameter( "pf_z", rclcpp::ParameterValue( 0.99 ) );
add_parameter(
"recovery_alpha_fast", rclcpp::ParameterValue( 0.0 ),
"Exponential decay rate for the fast average weight filter, used in deciding when to recover "
"by adding random poses",
"A good value might be 0.1" );
add_parameter(
"recovery_alpha_slow", rclcpp::ParameterValue( 0.0 ),
"Exponential decay rate for the slow average weight filter, used in deciding when to recover "
"by adding random poses",
"A good value might be 0.001" );
add_parameter(
"resample_interval", rclcpp::ParameterValue( 1 ),
"Number of filter updates required before resampling" );
add_parameter( "robot_model_type", rclcpp::ParameterValue( "nav2_amcl::DifferentialMotionModel" ) );
add_parameter(
"save_pose_rate", rclcpp::ParameterValue( 0.5 ),
"Maximum rate ( Hz ) at which to store the last estimated pose and covariance to the parameter "
"server, in the variables ~initial_pose_* and ~initial_cov_*. This saved pose will be used "
"on subsequent runs to initialize the filter",
"-1.0 to disable" );
add_parameter( "sigma_hit", rclcpp::ParameterValue( 0.2 ) );
add_parameter(
"tf_broadcast", rclcpp::ParameterValue( true ),
"Set this to false to prevent amcl from publishing the transform between the global frame and "
"the odometry frame" );
add_parameter(
"transform_tolerance", rclcpp::ParameterValue( 1.0 ),
"Time with which to post-date the transform that is published, to indicate that this transform "
"is valid into the future" );
add_parameter(
"update_min_a", rclcpp::ParameterValue( 0.2 ),
"Rotational movement required before performing a filter update" );
add_parameter(
"update_min_d", rclcpp::ParameterValue( 0.25 ),
"Translational movement required before performing a filter update" );
add_parameter( "z_hit", rclcpp::ParameterValue( 0.5 ) );
add_parameter( "z_max", rclcpp::ParameterValue( 0.05 ) );
add_parameter( "z_rand", rclcpp::ParameterValue( 0.5 ) );
add_parameter( "z_short", rclcpp::ParameterValue( 0.05 ) );
add_parameter(
"always_reset_initial_pose", rclcpp::ParameterValue( false ),
"Requires that AMCL is provided an initial pose either via topic or initial_pose* parameter "
"( with parameter set_initial_pose: true ) when reset. Otherwise, by default AMCL will use the"
"last known pose to initialize" );
add_parameter(
"scan_topic", rclcpp::ParameterValue( "scan" ),
"Topic to subscribe to in order to receive the laser scan for localization" );
add_parameter(
"map_topic", rclcpp::ParameterValue( "map" ),
"Topic to subscribe to in order to receive the map to localize on" );
add_parameter(
"first_map_only", rclcpp::ParameterValue( false ),
"Set this to true, when you want to load a new map published from the map_server" );
}
231 AmclNode::~AmclNode( )
{
}
nav2_util::CallbackReturn
236 AmclNode::on_configure( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Configuring" );
callback_group_ = create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive, false );
initParameters( );
initTransforms( );
initParticleFilter( );
initLaserScan( );
initMessageFilters( );
initPubSub( );
initServices( );
initOdometry( );
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>( );
executor_->add_callback_group( callback_group_, get_node_base_interface( ) );
executor_thread_ = std::make_unique<nav2_util::NodeThread>( executor_ );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
256 AmclNode::on_activate( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Activating" );
// Lifecycle publishers must be explicitly activated
pose_pub_->on_activate( );
particle_cloud_pub_->on_activate( );
first_pose_sent_ = false;
// Keep track of whether we're in the active state. We won't
// process incoming callbacks until we are
active_ = true;
if ( set_initial_pose_ ) {
auto msg = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>( );
msg->header.stamp = now( );
msg->header.frame_id = global_frame_id_;
msg->pose.pose.position.x = initial_pose_x_;
msg->pose.pose.position.y = initial_pose_y_;
msg->pose.pose.position.z = initial_pose_z_;
msg->pose.pose.orientation = orientationAroundZAxis( initial_pose_yaw_ );
initialPoseReceived( msg );
} else if ( init_pose_received_on_inactive ) {
handleInitialPose( last_published_pose_ );
}
auto node = shared_from_this( );
// Add callback for dynamic parameters
dyn_params_handler_ = node->add_on_set_parameters_callback(
std::bind(
&AmclNode::dynamicParametersCallback,
this, std::placeholders::_1 ) );
// create bond connection
createBond( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
299 AmclNode::on_deactivate( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Deactivating" );
active_ = false;
// Lifecycle publishers must be explicitly deactivated
pose_pub_->on_deactivate( );
particle_cloud_pub_->on_deactivate( );
// reset dynamic parameter handler
dyn_params_handler_.reset( );
// destroy bond connection
destroyBond( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
319 AmclNode::on_cleanup( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Cleaning up" );
executor_thread_.reset( );
// Get rid of the inputs first ( services and message filter input ), so we
// don't continue to process incoming messages
global_loc_srv_.reset( );
nomotion_update_srv_.reset( );
initial_pose_sub_.reset( );
laser_scan_connection_.disconnect( );
laser_scan_filter_.reset( );
laser_scan_sub_.reset( );
// Map
if ( map_ != NULL ) {
map_free( map_ );
map_ = nullptr;
}
first_map_received_ = false;
free_space_indices.resize( 0 );
// Transforms
tf_broadcaster_.reset( );
tf_listener_.reset( );
tf_buffer_.reset( );
// PubSub
pose_pub_.reset( );
particle_cloud_pub_.reset( );
// Odometry
motion_model_.reset( );
// Particle Filter
pf_free( pf_ );
pf_ = nullptr;
// Laser Scan
lasers_.clear( );
lasers_update_.clear( );
frame_to_laser_.clear( );
force_update_ = true;
if ( set_initial_pose_ ) {
set_parameter(
rclcpp::Parameter(
"initial_pose.x",
rclcpp::ParameterValue( last_published_pose_.pose.pose.position.x ) ) );
set_parameter(
rclcpp::Parameter(
"initial_pose.y",
rclcpp::ParameterValue( last_published_pose_.pose.pose.position.y ) ) );
set_parameter(
rclcpp::Parameter(
"initial_pose.z",
rclcpp::ParameterValue( last_published_pose_.pose.pose.position.z ) ) );
set_parameter(
rclcpp::Parameter(
"initial_pose.yaw",
rclcpp::ParameterValue( tf2::getYaw( last_published_pose_.pose.pose.orientation ) ) ) );
}
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
387 AmclNode::on_shutdown( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Shutting down" );
return nav2_util::CallbackReturn::SUCCESS;
}
bool
394 AmclNode::checkElapsedTime( std::chrono::seconds check_interval, rclcpp::Time last_time )
{
rclcpp::Duration elapsed_time = now( ) - last_time;
if ( elapsed_time.nanoseconds( ) * 1e-9 > check_interval.count( ) ) {
return true;
}
return false;
}
#if NEW_UNIFORM_SAMPLING
404 std::vector<std::pair<int, int>> AmclNode::free_space_indices;
#endif
bool
408 AmclNode::getOdomPose(
409 geometry_msgs::msg::PoseStamped & odom_pose,
double & x, double & y, double & yaw,
411 const rclcpp::Time & sensor_timestamp, const std::string & frame_id )
{
// Get the robot's pose
geometry_msgs::msg::PoseStamped ident;
ident.header.frame_id = nav2_util::strip_leading_slash( frame_id );
ident.header.stamp = sensor_timestamp;
tf2::toMsg( tf2::Transform::getIdentity( ), ident.pose );
try {
tf_buffer_->transform( ident, odom_pose, odom_frame_id_ );
} catch ( tf2::TransformException & e ) {
++scan_error_count_;
if ( scan_error_count_ % 20 == 0 ) {
RCLCPP_ERROR(
get_logger( ), "( %d ) consecutive laser scan transforms failed: ( %s )", scan_error_count_,
e.what( ) );
}
return false;
}
scan_error_count_ = 0; // reset since we got a good transform
x = odom_pose.pose.position.x;
y = odom_pose.pose.position.y;
yaw = tf2::getYaw( odom_pose.pose.orientation );
return true;
}
pf_vector_t
440 AmclNode::uniformPoseGenerator( void * arg )
{
map_t * map = reinterpret_cast<map_t *>( arg );
#if NEW_UNIFORM_SAMPLING
unsigned int rand_index = drand48( ) * free_space_indices.size( );
std::pair<int, int> free_point = free_space_indices[rand_index];
pf_vector_t p;
p.v[0] = MAP_WXGX( map, free_point.first );
p.v[1] = MAP_WYGY( map, free_point.second );
p.v[2] = drand48( ) * 2 * M_PI - M_PI;
#else
double min_x, max_x, min_y, max_y;
min_x = ( map->size_x * map->scale ) / 2.0 - map->origin_x;
max_x = ( map->size_x * map->scale ) / 2.0 + map->origin_x;
min_y = ( map->size_y * map->scale ) / 2.0 - map->origin_y;
max_y = ( map->size_y * map->scale ) / 2.0 + map->origin_y;
pf_vector_t p;
RCLCPP_DEBUG( get_logger( ), "Generating new uniform sample" );
for ( ;; ) {
p.v[0] = min_x + drand48( ) * ( max_x - min_x );
p.v[1] = min_y + drand48( ) * ( max_y - min_y );
p.v[2] = drand48( ) * 2 * M_PI - M_PI;
// Check that it's a free cell
int i, j;
i = MAP_GXWX( map, p.v[0] );
j = MAP_GYWY( map, p.v[1] );
if ( MAP_VALID( map, i, j ) && ( map->cells[MAP_INDEX( map, i, j )].occ_state == -1 ) ) {
break;
}
}
#endif
return p;
}
void
479 AmclNode::globalLocalizationCallback(
480 const std::shared_ptr<rmw_request_id_t>/*request_header*/,
481 const std::shared_ptr<std_srvs::srv::Empty::Request>/*req*/,
482 std::shared_ptr<std_srvs::srv::Empty::Response>/*res*/ )
{
std::lock_guard<std::recursive_mutex> cfl( mutex_ );
RCLCPP_INFO( get_logger( ), "Initializing with uniform distribution" );
pf_init_model(
pf_, ( pf_init_model_fn_t )AmclNode::uniformPoseGenerator,
reinterpret_cast<void *>( map_ ) );
RCLCPP_INFO( get_logger( ), "Global initialisation done!" );
initial_pose_is_known_ = true;
pf_init_ = false;
}
// force nomotion updates ( amcl updating without requiring motion )
void
498 AmclNode::nomotionUpdateCallback(
499 const std::shared_ptr<rmw_request_id_t>/*request_header*/,
500 const std::shared_ptr<std_srvs::srv::Empty::Request>/*req*/,
501 std::shared_ptr<std_srvs::srv::Empty::Response>/*res*/ )
{
RCLCPP_INFO( get_logger( ), "Requesting no-motion update" );
force_update_ = true;
}
void
508 AmclNode::initialPoseReceived( geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg )
{
std::lock_guard<std::recursive_mutex> cfl( mutex_ );
RCLCPP_INFO( get_logger( ), "initialPoseReceived" );
if ( msg->header.frame_id == "" ) {
// This should be removed at some point
RCLCPP_WARN(
get_logger( ),
"Received initial pose with empty frame_id. You should always supply a frame_id." );
return;
}
if ( nav2_util::strip_leading_slash( msg->header.frame_id ) != global_frame_id_ ) {
RCLCPP_WARN(
get_logger( ),
"Ignoring initial pose in frame \"%s\"; initial poses must be in the global frame, \"%s\"",
nav2_util::strip_leading_slash( msg->header.frame_id ).c_str( ),
global_frame_id_.c_str( ) );
return;
}
// Overriding last published pose to initial pose
last_published_pose_ = *msg;
if ( !active_ ) {
init_pose_received_on_inactive = true;
RCLCPP_WARN(
get_logger( ), "Received initial pose request, "
"but AMCL is not yet in the active state" );
return;
}
handleInitialPose( *msg );
}
void
543 AmclNode::handleInitialPose( geometry_msgs::msg::PoseWithCovarianceStamped & msg )
{
std::lock_guard<std::recursive_mutex> cfl( mutex_ );
// In case the client sent us a pose estimate in the past, integrate the
// intervening odometric change.
geometry_msgs::msg::TransformStamped tx_odom;
try {
rclcpp::Time rclcpp_time = now( );
tf2::TimePoint tf2_time( std::chrono::nanoseconds( rclcpp_time.nanoseconds( ) ) );
// Check if the transform is available
tx_odom = tf_buffer_->lookupTransform(
base_frame_id_, tf2_ros::fromMsg( msg.header.stamp ),
base_frame_id_, tf2_time, odom_frame_id_ );
} catch ( tf2::TransformException & e ) {
// If we've never sent a transform, then this is normal, because the
// global_frame_id_ frame doesn't exist. We only care about in-time
// transformation for on-the-move pose-setting, so ignoring this
// startup condition doesn't really cost us anything.
if ( sent_first_transform_ ) {
RCLCPP_WARN( get_logger( ), "Failed to transform initial pose in time ( %s )", e.what( ) );
}
tf2::impl::Converter<false, true>::convert( tf2::Transform::getIdentity( ), tx_odom.transform );
}
tf2::Transform tx_odom_tf2;
tf2::impl::Converter<true, false>::convert( tx_odom.transform, tx_odom_tf2 );
tf2::Transform pose_old;
tf2::impl::Converter<true, false>::convert( msg.pose.pose, pose_old );
tf2::Transform pose_new = pose_old * tx_odom_tf2;
// Transform into the global frame
RCLCPP_INFO(
get_logger( ), "Setting pose ( %.6f ): %.3f %.3f %.3f",
now( ).nanoseconds( ) * 1e-9,
pose_new.getOrigin( ).x( ),
pose_new.getOrigin( ).y( ),
tf2::getYaw( pose_new.getRotation( ) ) );
// Re-initialize the filter
pf_vector_t pf_init_pose_mean = pf_vector_zero( );
pf_init_pose_mean.v[0] = pose_new.getOrigin( ).x( );
pf_init_pose_mean.v[1] = pose_new.getOrigin( ).y( );
pf_init_pose_mean.v[2] = tf2::getYaw( pose_new.getRotation( ) );
pf_matrix_t pf_init_pose_cov = pf_matrix_zero( );
// Copy in the covariance, converting from 6-D to 3-D
for ( int i = 0; i < 2; i++ ) {
for ( int j = 0; j < 2; j++ ) {
pf_init_pose_cov.m[i][j] = msg.pose.covariance[6 * i + j];
}
}
pf_init_pose_cov.m[2][2] = msg.pose.covariance[6 * 5 + 5];
pf_init( pf_, pf_init_pose_mean, pf_init_pose_cov );
pf_init_ = false;
init_pose_received_on_inactive = false;
initial_pose_is_known_ = true;
}
void
608 AmclNode::laserReceived( sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan )
{
std::lock_guard<std::recursive_mutex> cfl( mutex_ );
// Since the sensor data is continually being published by the simulator or robot,
// we don't want our callbacks to fire until we're in the active state
if ( !active_ ) {return;}
if ( !first_map_received_ ) {
if ( checkElapsedTime( 2s, last_time_printed_msg_ ) ) {
RCLCPP_WARN( get_logger( ), "Waiting for map...." );
last_time_printed_msg_ = now( );
}
return;
}
std::string laser_scan_frame_id = nav2_util::strip_leading_slash( laser_scan->header.frame_id );
last_laser_received_ts_ = now( );
int laser_index = -1;
geometry_msgs::msg::PoseStamped laser_pose;
// Do we have the base->base_laser Tx yet?
if ( frame_to_laser_.find( laser_scan_frame_id ) == frame_to_laser_.end( ) ) {
if ( !addNewScanner( laser_index, laser_scan, laser_scan_frame_id, laser_pose ) ) {
return; // could not find transform
}
} else {
// we have the laser pose, retrieve laser index
laser_index = frame_to_laser_[laser_scan->header.frame_id];
}
// Where was the robot when this scan was taken?
pf_vector_t pose;
if ( !getOdomPose(
latest_odom_pose_, pose.v[0], pose.v[1], pose.v[2],
laser_scan->header.stamp, base_frame_id_ ) )
{
RCLCPP_ERROR( get_logger( ), "Couldn't determine robot's pose associated with laser scan" );
return;
}
pf_vector_t delta = pf_vector_zero( );
bool force_publication = false;
if ( !pf_init_ ) {
// Pose at last filter update
pf_odom_pose_ = pose;
pf_init_ = true;
for ( unsigned int i = 0; i < lasers_update_.size( ); i++ ) {
lasers_update_[i] = true;
}
force_publication = true;
resample_count_ = 0;
} else {
// Set the laser update flags
if ( shouldUpdateFilter( pose, delta ) ) {
for ( unsigned int i = 0; i < lasers_update_.size( ); i++ ) {
lasers_update_[i] = true;
}
}
if ( lasers_update_[laser_index] ) {
motion_model_->odometryUpdate( pf_, pose, delta );
}
force_update_ = false;
}
bool resampled = false;
// If the robot has moved, update the filter
if ( lasers_update_[laser_index] ) {
updateFilter( laser_index, laser_scan, pose );
// Resample the particles
if ( !( ++resample_count_ % resample_interval_ ) ) {
pf_update_resample( pf_ );
resampled = true;
}
pf_sample_set_t * set = pf_->sets + pf_->current_set;
RCLCPP_DEBUG( get_logger( ), "Num samples: %d\n", set->sample_count );
if ( !force_update_ ) {
publishParticleCloud( set );
}
}
if ( resampled || force_publication || !first_pose_sent_ ) {
amcl_hyp_t max_weight_hyps;
std::vector<amcl_hyp_t> hyps;
int max_weight_hyp = -1;
if ( getMaxWeightHyp( hyps, max_weight_hyps, max_weight_hyp ) ) {
publishAmclPose( laser_scan, hyps, max_weight_hyp );
calculateMaptoOdomTransform( laser_scan, hyps, max_weight_hyp );
if ( tf_broadcast_ == true ) {
// We want to send a transform that is good up until a
// tolerance time so that odom can be used
auto stamp = tf2_ros::fromMsg( laser_scan->header.stamp );
tf2::TimePoint transform_expiration = stamp + transform_tolerance_;
sendMapToOdomTransform( transform_expiration );
sent_first_transform_ = true;
}
} else {
RCLCPP_ERROR( get_logger( ), "No pose!" );
}
} else if ( latest_tf_valid_ ) {
if ( tf_broadcast_ == true ) {
// Nothing changed, so we'll just republish the last transform, to keep
// everybody happy.
tf2::TimePoint transform_expiration = tf2_ros::fromMsg( laser_scan->header.stamp ) +
transform_tolerance_;
sendMapToOdomTransform( transform_expiration );
}
}
}
723 bool AmclNode::addNewScanner(
int & laser_index,
725 const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
726 const std::string & laser_scan_frame_id,
727 geometry_msgs::msg::PoseStamped & laser_pose )
{
lasers_.push_back( createLaserObject( ) );
lasers_update_.push_back( true );
laser_index = frame_to_laser_.size( );
geometry_msgs::msg::PoseStamped ident;
ident.header.frame_id = laser_scan_frame_id;
ident.header.stamp = rclcpp::Time( );
tf2::toMsg( tf2::Transform::getIdentity( ), ident.pose );
try {
tf_buffer_->transform( ident, laser_pose, base_frame_id_, transform_tolerance_ );
} catch ( tf2::TransformException & e ) {
RCLCPP_ERROR(
get_logger( ), "Couldn't transform from %s to %s, "
"even though the message notifier is in use: ( %s )",
laser_scan->header.frame_id.c_str( ),
base_frame_id_.c_str( ), e.what( ) );
return false;
}
pf_vector_t laser_pose_v;
laser_pose_v.v[0] = laser_pose.pose.position.x;
laser_pose_v.v[1] = laser_pose.pose.position.y;
// laser mounting angle gets computed later -> set to 0 here!
laser_pose_v.v[2] = 0;
lasers_[laser_index]->SetLaserPose( laser_pose_v );
frame_to_laser_[laser_scan->header.frame_id] = laser_index;
return true;
}
758 bool AmclNode::shouldUpdateFilter( const pf_vector_t pose, pf_vector_t & delta )
{
delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
delta.v[2] = angleutils::angle_diff( pose.v[2], pf_odom_pose_.v[2] );
// See if we should update the filter
bool update = fabs( delta.v[0] ) > d_thresh_ ||
fabs( delta.v[1] ) > d_thresh_ ||
fabs( delta.v[2] ) > a_thresh_;
update = update || force_update_;
return update;
}
772 bool AmclNode::updateFilter(
const int & laser_index,
774 const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
775 const pf_vector_t & pose )
{
nav2_amcl::LaserData ldata;
ldata.laser = lasers_[laser_index];
ldata.range_count = laser_scan->ranges.size( );
// To account for lasers that are mounted upside-down, we determine the
// min, max, and increment angles of the laser in the base frame.
//
// Construct min and max angles of laser, in the base_link frame.
// Here we set the roll pich yaw of the lasers. We assume roll and pich are zero.
geometry_msgs::msg::QuaternionStamped min_q, inc_q;
min_q.header.stamp = laser_scan->header.stamp;
min_q.header.frame_id = nav2_util::strip_leading_slash( laser_scan->header.frame_id );
min_q.quaternion = orientationAroundZAxis( laser_scan->angle_min );
inc_q.header = min_q.header;
inc_q.quaternion = orientationAroundZAxis( laser_scan->angle_min + laser_scan->angle_increment );
try {
tf_buffer_->transform( min_q, min_q, base_frame_id_ );
tf_buffer_->transform( inc_q, inc_q, base_frame_id_ );
} catch ( tf2::TransformException & e ) {
RCLCPP_WARN(
get_logger( ), "Unable to transform min/max laser angles into base frame: %s",
e.what( ) );
return false;
}
double angle_min = tf2::getYaw( min_q.quaternion );
double angle_increment = tf2::getYaw( inc_q.quaternion ) - angle_min;
// wrapping angle to [-pi .. pi]
angle_increment = fmod( angle_increment + 5 * M_PI, 2 * M_PI ) - M_PI;
RCLCPP_DEBUG(
get_logger( ), "Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min,
angle_increment );
// Apply range min/max thresholds, if the user supplied them
if ( laser_max_range_ > 0.0 ) {
ldata.range_max = std::min( laser_scan->range_max, static_cast<float>( laser_max_range_ ) );
} else {
ldata.range_max = laser_scan->range_max;
}
double range_min;
if ( laser_min_range_ > 0.0 ) {
range_min = std::max( laser_scan->range_min, static_cast<float>( laser_min_range_ ) );
} else {
range_min = laser_scan->range_min;
}
// The LaserData destructor will free this memory
ldata.ranges = new double[ldata.range_count][2];
for ( int i = 0; i < ldata.range_count; i++ ) {
// amcl doesn't ( yet ) have a concept of min range. So we'll map short
// readings to max range.
if ( laser_scan->ranges[i] <= range_min ) {
ldata.ranges[i][0] = ldata.range_max;
} else {
ldata.ranges[i][0] = laser_scan->ranges[i];
}
// Compute bearing
ldata.ranges[i][1] = angle_min +
( i * angle_increment );
}
lasers_[laser_index]->sensorUpdate( pf_, reinterpret_cast<nav2_amcl::LaserData *>( &ldata ) );
lasers_update_[laser_index] = false;
pf_odom_pose_ = pose;
return true;
}
void
845 AmclNode::publishParticleCloud( const pf_sample_set_t * set )
{
// If initial pose is not known, AMCL does not know the current pose
if ( !initial_pose_is_known_ ) {return;}
auto cloud_with_weights_msg = std::make_unique<nav2_msgs::msg::ParticleCloud>( );
cloud_with_weights_msg->header.stamp = this->now( );
cloud_with_weights_msg->header.frame_id = global_frame_id_;
cloud_with_weights_msg->particles.resize( set->sample_count );
for ( int i = 0; i < set->sample_count; i++ ) {
cloud_with_weights_msg->particles[i].pose.position.x = set->samples[i].pose.v[0];
cloud_with_weights_msg->particles[i].pose.position.y = set->samples[i].pose.v[1];
cloud_with_weights_msg->particles[i].pose.position.z = 0;
cloud_with_weights_msg->particles[i].pose.orientation = orientationAroundZAxis(
set->samples[i].pose.v[2] );
cloud_with_weights_msg->particles[i].weight = set->samples[i].weight;
}
particle_cloud_pub_->publish( std::move( cloud_with_weights_msg ) );
}
bool
867 AmclNode::getMaxWeightHyp(
868 std::vector<amcl_hyp_t> & hyps, amcl_hyp_t & max_weight_hyps,
int & max_weight_hyp )
{
// Read out the current hypotheses
double max_weight = 0.0;
hyps.resize( pf_->sets[pf_->current_set].cluster_count );
for ( int hyp_count = 0;
hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++ )
{
double weight;
pf_vector_t pose_mean;
pf_matrix_t pose_cov;
if ( !pf_get_cluster_stats( pf_, hyp_count, &weight, &pose_mean, &pose_cov ) ) {
RCLCPP_ERROR( get_logger( ), "Couldn't get stats on cluster %d", hyp_count );
return false;
}
hyps[hyp_count].weight = weight;
hyps[hyp_count].pf_pose_mean = pose_mean;
hyps[hyp_count].pf_pose_cov = pose_cov;
if ( hyps[hyp_count].weight > max_weight ) {
max_weight = hyps[hyp_count].weight;
max_weight_hyp = hyp_count;
}
}
if ( max_weight > 0.0 ) {
RCLCPP_DEBUG(
get_logger( ), "Max weight pose: %.3f %.3f %.3f",
hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
hyps[max_weight_hyp].pf_pose_mean.v[2] );
max_weight_hyps = hyps[max_weight_hyp];
return true;
}
return false;
}
void
909 AmclNode::publishAmclPose(
910 const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
911 const std::vector<amcl_hyp_t> & hyps, const int & max_weight_hyp )
{
// If initial pose is not known, AMCL does not know the current pose
if ( !initial_pose_is_known_ ) {
if ( checkElapsedTime( 2s, last_time_printed_msg_ ) ) {
RCLCPP_WARN(
get_logger( ), "AMCL cannot publish a pose or update the transform. "
"Please set the initial pose..." );
last_time_printed_msg_ = now( );
}
return;
}
auto p = std::make_unique<geometry_msgs::msg::PoseWithCovarianceStamped>( );
// Fill in the header
p->header.frame_id = global_frame_id_;
p->header.stamp = laser_scan->header.stamp;
// Copy in the pose
p->pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
p->pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
p->pose.pose.orientation = orientationAroundZAxis( hyps[max_weight_hyp].pf_pose_mean.v[2] );
// Copy in the covariance, converting from 3-D to 6-D
pf_sample_set_t * set = pf_->sets + pf_->current_set;
for ( int i = 0; i < 2; i++ ) {
for ( int j = 0; j < 2; j++ ) {
// Report the overall filter covariance, rather than the
// covariance for the highest-weight cluster
// p->covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j];
p->pose.covariance[6 * i + j] = set->cov.m[i][j];
}
}
p->pose.covariance[6 * 5 + 5] = set->cov.m[2][2];
float temp = 0.0;
for ( auto covariance_value : p->pose.covariance ) {
temp += covariance_value;
}
temp += p->pose.pose.position.x + p->pose.pose.position.y;
if ( !std::isnan( temp ) ) {
RCLCPP_DEBUG( get_logger( ), "Publishing pose" );
last_published_pose_ = *p;
first_pose_sent_ = true;
pose_pub_->publish( std::move( p ) );
} else {
RCLCPP_WARN(
get_logger( ), "AMCL covariance or pose is NaN, likely due to an invalid "
"configuration or faulty sensor measurements! Pose is not available!" );
}
RCLCPP_DEBUG(
get_logger( ), "New pose: %6.3f %6.3f %6.3f",
hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
hyps[max_weight_hyp].pf_pose_mean.v[2] );
}
void
967 AmclNode::calculateMaptoOdomTransform(
968 const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
969 const std::vector<amcl_hyp_t> & hyps, const int & max_weight_hyp )
{
// subtracting base to odom from map to base and send map to odom instead
geometry_msgs::msg::PoseStamped odom_to_map;
try {
tf2::Quaternion q;
q.setRPY( 0, 0, hyps[max_weight_hyp].pf_pose_mean.v[2] );
tf2::Transform tmp_tf( q, tf2::Vector3(
hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
0.0 ) );
geometry_msgs::msg::PoseStamped tmp_tf_stamped;
tmp_tf_stamped.header.frame_id = base_frame_id_;
tmp_tf_stamped.header.stamp = laser_scan->header.stamp;
tf2::toMsg( tmp_tf.inverse( ), tmp_tf_stamped.pose );
tf_buffer_->transform( tmp_tf_stamped, odom_to_map, odom_frame_id_ );
} catch ( tf2::TransformException & e ) {
RCLCPP_DEBUG( get_logger( ), "Failed to subtract base to odom transform: ( %s )", e.what( ) );
return;
}
tf2::impl::Converter<true, false>::convert( odom_to_map.pose, latest_tf_ );
latest_tf_valid_ = true;
}
void
997 AmclNode::sendMapToOdomTransform( const tf2::TimePoint & transform_expiration )
{
// AMCL will update transform only when it has knowledge about robot's initial position
if ( !initial_pose_is_known_ ) {return;}
geometry_msgs::msg::TransformStamped tmp_tf_stamped;
tmp_tf_stamped.header.frame_id = global_frame_id_;
tmp_tf_stamped.header.stamp = tf2_ros::toMsg( transform_expiration );
tmp_tf_stamped.child_frame_id = odom_frame_id_;
tf2::impl::Converter<false, true>::convert( latest_tf_.inverse( ), tmp_tf_stamped.transform );
tf_broadcaster_->sendTransform( tmp_tf_stamped );
}
nav2_amcl::Laser *
1010 AmclNode::createLaserObject( )
{
RCLCPP_INFO( get_logger( ), "createLaserObject" );
if ( sensor_model_type_ == "beam" ) {
return new nav2_amcl::BeamModel(
z_hit_, z_short_, z_max_, z_rand_, sigma_hit_, lambda_short_,
0.0, max_beams_, map_ );
}
if ( sensor_model_type_ == "likelihood_field_prob" ) {
return new nav2_amcl::LikelihoodFieldModelProb(
z_hit_, z_rand_, sigma_hit_,
laser_likelihood_max_dist_, do_beamskip_, beam_skip_distance_, beam_skip_threshold_,
beam_skip_error_threshold_, max_beams_, map_ );
}
return new nav2_amcl::LikelihoodFieldModel(
z_hit_, z_rand_, sigma_hit_,
laser_likelihood_max_dist_, max_beams_, map_ );
}
void
1033 AmclNode::initParameters( )
{
double save_pose_rate;
double tmp_tol;
get_parameter( "alpha1", alpha1_ );
get_parameter( "alpha2", alpha2_ );
get_parameter( "alpha3", alpha3_ );
get_parameter( "alpha4", alpha4_ );
get_parameter( "alpha5", alpha5_ );
get_parameter( "base_frame_id", base_frame_id_ );
get_parameter( "beam_skip_distance", beam_skip_distance_ );
get_parameter( "beam_skip_error_threshold", beam_skip_error_threshold_ );
get_parameter( "beam_skip_threshold", beam_skip_threshold_ );
get_parameter( "do_beamskip", do_beamskip_ );
get_parameter( "global_frame_id", global_frame_id_ );
get_parameter( "lambda_short", lambda_short_ );
get_parameter( "laser_likelihood_max_dist", laser_likelihood_max_dist_ );
get_parameter( "laser_max_range", laser_max_range_ );
get_parameter( "laser_min_range", laser_min_range_ );
get_parameter( "laser_model_type", sensor_model_type_ );
get_parameter( "set_initial_pose", set_initial_pose_ );
get_parameter( "initial_pose.x", initial_pose_x_ );
get_parameter( "initial_pose.y", initial_pose_y_ );
get_parameter( "initial_pose.z", initial_pose_z_ );
get_parameter( "initial_pose.yaw", initial_pose_yaw_ );
get_parameter( "max_beams", max_beams_ );
get_parameter( "max_particles", max_particles_ );
get_parameter( "min_particles", min_particles_ );
get_parameter( "odom_frame_id", odom_frame_id_ );
get_parameter( "pf_err", pf_err_ );
get_parameter( "pf_z", pf_z_ );
get_parameter( "recovery_alpha_fast", alpha_fast_ );
get_parameter( "recovery_alpha_slow", alpha_slow_ );
get_parameter( "resample_interval", resample_interval_ );
get_parameter( "robot_model_type", robot_model_type_ );
get_parameter( "save_pose_rate", save_pose_rate );
get_parameter( "sigma_hit", sigma_hit_ );
get_parameter( "tf_broadcast", tf_broadcast_ );
get_parameter( "transform_tolerance", tmp_tol );
get_parameter( "update_min_a", a_thresh_ );
get_parameter( "update_min_d", d_thresh_ );
get_parameter( "z_hit", z_hit_ );
get_parameter( "z_max", z_max_ );
get_parameter( "z_rand", z_rand_ );
get_parameter( "z_short", z_short_ );
get_parameter( "first_map_only", first_map_only_ );
get_parameter( "always_reset_initial_pose", always_reset_initial_pose_ );
get_parameter( "scan_topic", scan_topic_ );
get_parameter( "map_topic", map_topic_ );
save_pose_period_ = tf2::durationFromSec( 1.0 / save_pose_rate );
transform_tolerance_ = tf2::durationFromSec( tmp_tol );
odom_frame_id_ = nav2_util::strip_leading_slash( odom_frame_id_ );
base_frame_id_ = nav2_util::strip_leading_slash( base_frame_id_ );
global_frame_id_ = nav2_util::strip_leading_slash( global_frame_id_ );
last_time_printed_msg_ = now( );
// Semantic checks
if ( laser_likelihood_max_dist_ < 0 ) {
RCLCPP_WARN(
get_logger( ), "You've set laser_likelihood_max_dist to be negtive, "
" this isn't allowed so it will be set to default value 2.0." );
laser_likelihood_max_dist_ = 2.0;
}
if ( max_particles_ < 0 ) {
RCLCPP_WARN(
get_logger( ), "You've set max_particles to be negtive, "
" this isn't allowed so it will be set to default value 2000." );
max_particles_ = 2000;
}
if ( min_particles_ < 0 ) {
RCLCPP_WARN(
get_logger( ), "You've set min_particles to be negtive, "
" this isn't allowed so it will be set to default value 500." );
min_particles_ = 500;
}
if ( min_particles_ > max_particles_ ) {
RCLCPP_WARN(
get_logger( ), "You've set min_particles to be greater than max particles, "
" this isn't allowed so max_particles will be set to min_particles." );
max_particles_ = min_particles_;
}
if ( resample_interval_ <= 0 ) {
RCLCPP_WARN(
get_logger( ), "You've set resample_interval to be zero or negtive, "
" this isn't allowed so it will be set to default value to 1." );
resample_interval_ = 1;
}
if ( always_reset_initial_pose_ ) {
initial_pose_is_known_ = false;
}
}
/**
* @brief Callback executed when a parameter change is detected
* @param event ParameterEvent message
*/
rcl_interfaces::msg::SetParametersResult
1138 AmclNode::dynamicParametersCallback(
1139 std::vector<rclcpp::Parameter> parameters )
{
std::lock_guard<std::recursive_mutex> cfl( mutex_ );
rcl_interfaces::msg::SetParametersResult result;
double save_pose_rate;
double tmp_tol;
int max_particles = max_particles_;
int min_particles = min_particles_;
bool reinit_pf = false;
bool reinit_odom = false;
bool reinit_laser = false;
bool reinit_map = false;
for ( auto parameter : parameters ) {
const auto & param_type = parameter.get_type( );
const auto & param_name = parameter.get_name( );
if ( param_type == ParameterType::PARAMETER_DOUBLE ) {
if ( param_name == "alpha1" ) {
alpha1_ = parameter.as_double( );
reinit_odom = true;
} else if ( param_name == "alpha2" ) {
alpha2_ = parameter.as_double( );
reinit_odom = true;
} else if ( param_name == "alpha3" ) {
alpha3_ = parameter.as_double( );
reinit_odom = true;
} else if ( param_name == "alpha4" ) {
alpha4_ = parameter.as_double( );
reinit_odom = true;
} else if ( param_name == "alpha5" ) {
alpha5_ = parameter.as_double( );
reinit_odom = true;
} else if ( param_name == "beam_skip_distance" ) {
beam_skip_distance_ = parameter.as_double( );
reinit_laser = true;
} else if ( param_name == "beam_skip_error_threshold" ) {
beam_skip_error_threshold_ = parameter.as_double( );
reinit_laser = true;
} else if ( param_name == "beam_skip_threshold" ) {
beam_skip_threshold_ = parameter.as_double( );
reinit_laser = true;
} else if ( param_name == "lambda_short" ) {
lambda_short_ = parameter.as_double( );
reinit_laser = true;
} else if ( param_name == "laser_likelihood_max_dist" ) {
laser_likelihood_max_dist_ = parameter.as_double( );
reinit_laser = true;
} else if ( param_name == "laser_max_range" ) {
laser_max_range_ = parameter.as_double( );
reinit_laser = true;
} else if ( param_name == "laser_min_range" ) {
laser_min_range_ = parameter.as_double( );
reinit_laser = true;
} else if ( param_name == "pf_err" ) {
pf_err_ = parameter.as_double( );
reinit_pf = true;
} else if ( param_name == "pf_z" ) {
pf_z_ = parameter.as_double( );
reinit_pf = true;
} else if ( param_name == "recovery_alpha_fast" ) {
alpha_fast_ = parameter.as_double( );
reinit_pf = true;
} else if ( param_name == "recovery_alpha_slow" ) {
alpha_slow_ = parameter.as_double( );
reinit_pf = true;
} else if ( param_name == "save_pose_rate" ) {
save_pose_rate = parameter.as_double( );
save_pose_period_ = tf2::durationFromSec( 1.0 / save_pose_rate );
} else if ( param_name == "sigma_hit" ) {
sigma_hit_ = parameter.as_double( );
reinit_laser = true;
} else if ( param_name == "transform_tolerance" ) {
tmp_tol = parameter.as_double( );
transform_tolerance_ = tf2::durationFromSec( tmp_tol );
reinit_laser = true;
} else if ( param_name == "update_min_a" ) {
a_thresh_ = parameter.as_double( );
} else if ( param_name == "update_min_d" ) {
d_thresh_ = parameter.as_double( );
} else if ( param_name == "z_hit" ) {
z_hit_ = parameter.as_double( );
reinit_laser = true;
} else if ( param_name == "z_max" ) {
z_max_ = parameter.as_double( );
reinit_laser = true;
} else if ( param_name == "z_rand" ) {
z_rand_ = parameter.as_double( );
reinit_laser = true;
} else if ( param_name == "z_short" ) {
z_short_ = parameter.as_double( );
reinit_laser = true;
}
} else if ( param_type == ParameterType::PARAMETER_STRING ) {
if ( param_name == "base_frame_id" ) {
base_frame_id_ = parameter.as_string( );
} else if ( param_name == "global_frame_id" ) {
global_frame_id_ = parameter.as_string( );
} else if ( param_name == "map_topic" ) {
map_topic_ = parameter.as_string( );
reinit_map = true;
} else if ( param_name == "laser_model_type" ) {
sensor_model_type_ = parameter.as_string( );
reinit_laser = true;
} else if ( param_name == "odom_frame_id" ) {
odom_frame_id_ = parameter.as_string( );
reinit_laser = true;
} else if ( param_name == "scan_topic" ) {
scan_topic_ = parameter.as_string( );
reinit_laser = true;
} else if ( param_name == "robot_model_type" ) {
robot_model_type_ = parameter.as_string( );
reinit_odom = true;
}
} else if ( param_type == ParameterType::PARAMETER_BOOL ) {
if ( param_name == "do_beamskip" ) {
do_beamskip_ = parameter.as_bool( );
reinit_laser = true;
} else if ( param_name == "tf_broadcast" ) {
tf_broadcast_ = parameter.as_bool( );
} else if ( param_name == "set_initial_pose" ) {
set_initial_pose_ = parameter.as_bool( );
} else if ( param_name == "first_map_only" ) {
first_map_only_ = parameter.as_bool( );
}
} else if ( param_type == ParameterType::PARAMETER_INTEGER ) {
if ( param_name == "max_beams" ) {
max_beams_ = parameter.as_int( );
reinit_laser = true;
} else if ( param_name == "max_particles" ) {
max_particles_ = parameter.as_int( );
reinit_pf = true;
} else if ( param_name == "min_particles" ) {
min_particles_ = parameter.as_int( );
reinit_pf = true;
} else if ( param_name == "resample_interval" ) {
resample_interval_ = parameter.as_int( );
}
}
}
// Checking if the minimum particles is greater than max_particles_
if ( min_particles_ > max_particles_ ) {
RCLCPP_ERROR(
this->get_logger( ),
"You've set min_particles to be greater than max particles, "
" this isn't allowed." );
// sticking to the old values
max_particles_ = max_particles;
min_particles_ = min_particles;
result.successful = false;
return result;
}
// Re-initialize the particle filter
if ( reinit_pf ) {
if ( pf_ != NULL ) {
pf_free( pf_ );
pf_ = NULL;
}
initParticleFilter( );
}
// Re-initialize the odometry
if ( reinit_odom ) {
motion_model_.reset( );
initOdometry( );
}
// Re-initialize the lasers and it's filters
if ( reinit_laser ) {
lasers_.clear( );
lasers_update_.clear( );
frame_to_laser_.clear( );
laser_scan_connection_.disconnect( );
laser_scan_sub_.reset( );
initMessageFilters( );
}
// Re-initialize the map
if ( reinit_map ) {
map_sub_.reset( );
map_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
map_topic_, rclcpp::QoS( rclcpp::KeepLast( 1 ) ).transient_local( ).reliable( ),
std::bind( &AmclNode::mapReceived, this, std::placeholders::_1 ) );
}
result.successful = true;
return result;
}
void
1334 AmclNode::mapReceived( const nav_msgs::msg::OccupancyGrid::SharedPtr msg )
{
RCLCPP_DEBUG( get_logger( ), "AmclNode: A new map was received." );
if ( first_map_only_ && first_map_received_ ) {
return;
}
handleMapMessage( *msg );
first_map_received_ = true;
}
void
1345 AmclNode::handleMapMessage( const nav_msgs::msg::OccupancyGrid & msg )
{
std::lock_guard<std::recursive_mutex> cfl( mutex_ );
RCLCPP_INFO(
get_logger( ), "Received a %d X %d map @ %.3f m/pix",
msg.info.width,
msg.info.height,
msg.info.resolution );
if ( msg.header.frame_id != global_frame_id_ ) {
RCLCPP_WARN(
get_logger( ), "Frame_id of map received:'%s' doesn't match global_frame_id:'%s'. This could"
" cause issues with reading published topics",
msg.header.frame_id.c_str( ),
global_frame_id_.c_str( ) );
}
freeMapDependentMemory( );
map_ = convertMap( msg );
#if NEW_UNIFORM_SAMPLING
createFreeSpaceVector( );
#endif
}
void
1370 AmclNode::createFreeSpaceVector( )
{
// Index of free space
free_space_indices.resize( 0 );
for ( int i = 0; i < map_->size_x; i++ ) {
for ( int j = 0; j < map_->size_y; j++ ) {
if ( map_->cells[MAP_INDEX( map_, i, j )].occ_state == -1 ) {
free_space_indices.push_back( std::make_pair( i, j ) );
}
}
}
}
void
1384 AmclNode::freeMapDependentMemory( )
{
if ( map_ != NULL ) {
map_free( map_ );
map_ = NULL;
}
// Clear queued laser objects because they hold pointers to the existing
// map, #5202.
lasers_.clear( );
lasers_update_.clear( );
frame_to_laser_.clear( );
}
// Convert an OccupancyGrid map message into the internal representation. This function
// allocates a map_t and returns it.
map_t *
1401 AmclNode::convertMap( const nav_msgs::msg::OccupancyGrid & map_msg )
{
map_t * map = map_alloc( );
map->size_x = map_msg.info.width;
map->size_y = map_msg.info.height;
map->scale = map_msg.info.resolution;
map->origin_x = map_msg.info.origin.position.x + ( map->size_x / 2 ) * map->scale;
map->origin_y = map_msg.info.origin.position.y + ( map->size_y / 2 ) * map->scale;
map->cells =
reinterpret_cast<map_cell_t *>( malloc( sizeof( map_cell_t ) * map->size_x * map->size_y ) );
// Convert to player format
for ( int i = 0; i < map->size_x * map->size_y; i++ ) {
if ( map_msg.data[i] == 0 ) {
map->cells[i].occ_state = -1;
} else if ( map_msg.data[i] == 100 ) {
map->cells[i].occ_state = +1;
} else {
map->cells[i].occ_state = 0;
}
}
return map;
}
void
1429 AmclNode::initTransforms( )
{
RCLCPP_INFO( get_logger( ), "initTransforms" );
// Initialize transform listener and broadcaster
tf_buffer_ = std::make_shared<tf2_ros::Buffer>( get_clock( ) );
auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
get_node_base_interface( ),
get_node_timers_interface( ),
callback_group_ );
tf_buffer_->setCreateTimerInterface( timer_interface );
tf_listener_ = std::make_shared<tf2_ros::TransformListener>( *tf_buffer_ );
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>( shared_from_this( ) );
sent_first_transform_ = false;
latest_tf_valid_ = false;
latest_tf_ = tf2::Transform::getIdentity( );
}
void
1449 AmclNode::initMessageFilters( )
{
auto sub_opt = rclcpp::SubscriptionOptions( );
sub_opt.callback_group = callback_group_;
laser_scan_sub_ = std::make_unique<message_filters::Subscriber<sensor_msgs::msg::LaserScan,
rclcpp_lifecycle::LifecycleNode>>(
shared_from_this( ), scan_topic_, rmw_qos_profile_sensor_data, sub_opt );
laser_scan_filter_ = std::make_unique<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>>(
*laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10,
get_node_logging_interface( ),
get_node_clock_interface( ),
transform_tolerance_ );
laser_scan_connection_ = laser_scan_filter_->registerCallback(
std::bind(
&AmclNode::laserReceived,
this, std::placeholders::_1 ) );
}
void
1471 AmclNode::initPubSub( )
{
RCLCPP_INFO( get_logger( ), "initPubSub" );
particle_cloud_pub_ = create_publisher<nav2_msgs::msg::ParticleCloud>(
"particle_cloud",
rclcpp::SensorDataQoS( ) );
pose_pub_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
"amcl_pose",
rclcpp::QoS( rclcpp::KeepLast( 1 ) ).transient_local( ).reliable( ) );
initial_pose_sub_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
"initialpose", rclcpp::SystemDefaultsQoS( ),
std::bind( &AmclNode::initialPoseReceived, this, std::placeholders::_1 ) );
map_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
map_topic_, rclcpp::QoS( rclcpp::KeepLast( 1 ) ).transient_local( ).reliable( ),
std::bind( &AmclNode::mapReceived, this, std::placeholders::_1 ) );
RCLCPP_INFO( get_logger( ), "Subscribed to map topic." );
}
void
1495 AmclNode::initServices( )
{
global_loc_srv_ = create_service<std_srvs::srv::Empty>(
"reinitialize_global_localization",
std::bind( &AmclNode::globalLocalizationCallback, this, _1, _2, _3 ) );
nomotion_update_srv_ = create_service<std_srvs::srv::Empty>(
"request_nomotion_update",
std::bind( &AmclNode::nomotionUpdateCallback, this, _1, _2, _3 ) );
}
void
1507 AmclNode::initOdometry( )
{
// TODO( mjeronimo ): We should handle persistance of the last known pose of the robot. We could
// then read that pose here and initialize using that.
// When pausing and resuming, remember the last robot pose so we don't start at 0:0 again
init_pose_[0] = last_published_pose_.pose.pose.position.x;
init_pose_[1] = last_published_pose_.pose.pose.position.y;
init_pose_[2] = tf2::getYaw( last_published_pose_.pose.pose.orientation );
if ( !initial_pose_is_known_ ) {
init_cov_[0] = 0.5 * 0.5;
init_cov_[1] = 0.5 * 0.5;
init_cov_[2] = ( M_PI / 12.0 ) * ( M_PI / 12.0 );
} else {
init_cov_[0] = last_published_pose_.pose.covariance[0];
init_cov_[1] = last_published_pose_.pose.covariance[7];
init_cov_[2] = last_published_pose_.pose.covariance[35];
}
motion_model_ = plugin_loader_.createSharedInstance( robot_model_type_ );
motion_model_->initialize( alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ );
latest_odom_pose_ = geometry_msgs::msg::PoseStamped( );
}
void
1534 AmclNode::initParticleFilter( )
{
// Create the particle filter
pf_ = pf_alloc(
min_particles_, max_particles_, alpha_slow_, alpha_fast_,
( pf_init_model_fn_t )AmclNode::uniformPoseGenerator,
reinterpret_cast<void *>( map_ ) );
pf_->pop_err = pf_err_;
pf_->pop_z = pf_z_;
// Initialize the filter
pf_vector_t pf_init_pose_mean = pf_vector_zero( );
pf_init_pose_mean.v[0] = init_pose_[0];
pf_init_pose_mean.v[1] = init_pose_[1];
pf_init_pose_mean.v[2] = init_pose_[2];
pf_matrix_t pf_init_pose_cov = pf_matrix_zero( );
pf_init_pose_cov.m[0][0] = init_cov_[0];
pf_init_pose_cov.m[1][1] = init_cov_[1];
pf_init_pose_cov.m[2][2] = init_cov_[2];
pf_init( pf_, pf_init_pose_mean, pf_init_pose_cov );
pf_init_ = false;
resample_count_ = 0;
memset( &pf_odom_pose_, 0, sizeof( pf_odom_pose_ ) );
}
void
1563 AmclNode::initLaserScan( )
{
scan_error_count_ = 0;
last_laser_received_ts_ = rclcpp::Time( 0 );
}
} // namespace nav2_amcl
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE( nav2_amcl::AmclNode )
1 // Copyright ( c ) 2018 Intel Corporation
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or ( at your option ) any later version.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
#include <memory>
#include "nav2_amcl/amcl_node.hpp"
#include "rclcpp/rclcpp.hpp"
22 int main( int argc, char ** argv )
{
rclcpp::init( argc, argv );
auto node = std::make_shared<nav2_amcl::AmclNode>( );
rclcpp::spin( node->get_node_base_interface( ) );
rclcpp::shutdown( );
return 0;
}
1 /*
* Player - One Hell of a Robot Server
* Copyright ( C ) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Global map ( grid-based )
* Author: Andrew Howard
* Date: 6 Feb 2003
* CVS: $Id: map.c 1713 2003-08-23 04:03:43Z inspectorg $
**************************************************************************/
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include "nav2_amcl/map/map.hpp"
// Create a new map
38 map_t * map_alloc( void )
{
map_t * map;
map = ( map_t * ) malloc( sizeof( map_t ) );
// Assume we start at ( 0, 0 )
map->origin_x = 0;
map->origin_y = 0;
// Make the size odd
map->size_x = 0;
map->size_y = 0;
map->scale = 0;
// Allocate storage for main map
map->cells = ( map_cell_t * ) NULL;
return map;
}
// Destroy a map
61 void map_free( map_t * map )
{
free( map->cells );
free( map );
}
1 /*
* Player - One Hell of a Robot Server
* Copyright ( C ) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <queue>
#include "nav2_amcl/map/map.hpp"
/*
* @class CellData
* @brief Data about map cells
*/
32 class CellData
{
public:
35 map_t * map_;
unsigned int i_, j_;
unsigned int src_i_, src_j_;
};
/*
* @class CachedDistanceMap
* @brief Cached map with distances
*/
44 class CachedDistanceMap
{
public:
/*
* @brief CachedDistanceMap constructor
*/
50 CachedDistanceMap( double scale, double max_dist )
: distances_( NULL ), scale_( scale ), max_dist_( max_dist )
{
cell_radius_ = max_dist / scale;
distances_ = new double *[cell_radius_ + 2];
for ( int i = 0; i <= cell_radius_ + 1; i++ ) {
distances_[i] = new double[cell_radius_ + 2];
for ( int j = 0; j <= cell_radius_ + 1; j++ ) {
distances_[i][j] = sqrt( i * i + j * j );
}
}
}
/*
* @brief CachedDistanceMap destructor
*/
66 ~CachedDistanceMap( )
{
if ( distances_ ) {
for ( int i = 0; i <= cell_radius_ + 1; i++ ) {
delete[] distances_[i];
}
delete[] distances_;
}
}
double ** distances_;
double scale_;
double max_dist_;
int cell_radius_;
};
/*
* @brief operator<
*/
84 bool operator<( const CellData & a, const CellData & b )
{
return a.map_->cells[MAP_INDEX(
a.map_, a.i_,
a.j_ )].occ_dist > a.map_->cells[MAP_INDEX( b.map_, b.i_, b.j_ )].occ_dist;
}
/*
* @brief get_distance_map
* @param scale of cost information wrt distance
* @param max_dist Maximum distance to cache from occupied information
* @return Pointer to cached distance map
*/
CachedDistanceMap *
98 get_distance_map( double scale, double max_dist )
{
static CachedDistanceMap * cdm = NULL;
if ( !cdm || ( cdm->scale_ != scale ) || ( cdm->max_dist_ != max_dist ) ) {
if ( cdm ) {
delete cdm;
}
cdm = new CachedDistanceMap( scale, max_dist );
}
return cdm;
}
/*
* @brief enqueue cell data for caching
*/
115 void enqueue(
map_t * map, int i, int j,
int src_i, int src_j,
std::priority_queue<CellData> & Q,
CachedDistanceMap * cdm,
unsigned char * marked )
{
if ( marked[MAP_INDEX( map, i, j )] ) {
return;
}
int di = abs( i - src_i );
int dj = abs( j - src_j );
double distance = cdm->distances_[di][dj];
if ( distance > cdm->cell_radius_ ) {
return;
}
map->cells[MAP_INDEX( map, i, j )].occ_dist = distance * map->scale;
CellData cell;
cell.map_ = map;
cell.i_ = i;
cell.j_ = j;
cell.src_i_ = src_i;
cell.src_j_ = src_j;
Q.push( cell );
marked[MAP_INDEX( map, i, j )] = 1;
}
/*
* @brief Update the cspace distance values
* @param map Map to update
* @param max_occ_distance Maximum distance for occpuancy interest
*/
153 void map_update_cspace( map_t * map, double max_occ_dist )
{
unsigned char * marked;
std::priority_queue<CellData> Q;
marked = new unsigned char[map->size_x * map->size_y];
memset( marked, 0, sizeof( unsigned char ) * map->size_x * map->size_y );
map->max_occ_dist = max_occ_dist;
CachedDistanceMap * cdm = get_distance_map( map->scale, map->max_occ_dist );
// Enqueue all the obstacle cells
CellData cell;
cell.map_ = map;
for ( int i = 0; i < map->size_x; i++ ) {
cell.src_i_ = cell.i_ = i;
for ( int j = 0; j < map->size_y; j++ ) {
if ( map->cells[MAP_INDEX( map, i, j )].occ_state == +1 ) {
map->cells[MAP_INDEX( map, i, j )].occ_dist = 0.0;
cell.src_j_ = cell.j_ = j;
marked[MAP_INDEX( map, i, j )] = 1;
Q.push( cell );
} else {
map->cells[MAP_INDEX( map, i, j )].occ_dist = max_occ_dist;
}
}
}
while ( !Q.empty( ) ) {
CellData current_cell = Q.top( );
if ( current_cell.i_ > 0 ) {
enqueue(
map, current_cell.i_ - 1, current_cell.j_,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked );
}
if ( current_cell.j_ > 0 ) {
enqueue(
map, current_cell.i_, current_cell.j_ - 1,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked );
}
if ( static_cast<int>( current_cell.i_ ) < map->size_x - 1 ) {
enqueue(
map, current_cell.i_ + 1, current_cell.j_,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked );
}
if ( static_cast<int>( current_cell.j_ ) < map->size_y - 1 ) {
enqueue(
map, current_cell.i_, current_cell.j_ + 1,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked );
}
Q.pop( );
}
delete[] marked;
}
1 /*
* Player - One Hell of a Robot Server
* Copyright ( C ) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Local map GUI functions
* Author: Andrew Howard
* Date: 18 Jan 2003
* CVS: $Id: map_draw.c 7057 2008-10-02 00:44:06Z gbiggs $
**************************************************************************/
#pragma GCC diagnostic ignored "-Wpedantic"
#ifdef INCLUDE_RTKGUI
#include <errno.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <rtk.h>
#include "nav2_amcl/map/map.hpp"
////////////////////////////////////////////////////////////////////////////
// Draw the occupancy map
42 void map_draw_occ( map_t * map, rtk_fig_t * fig )
{
int i, j;
int col;
map_cell_t * cell;
uint16_t * image;
uint16_t * pixel;
image = malloc( map->size_x * map->size_y * sizeof( image[0] ) );
// Draw occupancy
for ( j = 0; j < map->size_y; j++ ) {
for ( i = 0; i < map->size_x; i++ ) {
cell = map->cells + MAP_INDEX( map, i, j );
pixel = image + ( j * map->size_x + i );
col = 127 - 127 * cell->occ_state;
*pixel = RTK_RGB16( col, col, col );
}
}
// Draw the entire occupancy map as an image
rtk_fig_image(
fig, map->origin_x, map->origin_y, 0,
map->scale, map->size_x, map->size_y, 16, image, NULL );
free( image );
}
////////////////////////////////////////////////////////////////////////////
// Draw the cspace map
74 void map_draw_cspace( map_t * map, rtk_fig_t * fig )
{
int i, j;
int col;
map_cell_t * cell;
uint16_t * image;
uint16_t * pixel;
image = malloc( map->size_x * map->size_y * sizeof( image[0] ) );
// Draw occupancy
for ( j = 0; j < map->size_y; j++ ) {
for ( i = 0; i < map->size_x; i++ ) {
cell = map->cells + MAP_INDEX( map, i, j );
pixel = image + ( j * map->size_x + i );
col = 255 * cell->occ_dist / map->max_occ_dist;
*pixel = RTK_RGB16( col, col, col );
}
}
// Draw the entire occupancy map as an image
rtk_fig_image(
fig, map->origin_x, map->origin_y, 0,
map->scale, map->size_x, map->size_y, 16, image, NULL );
free( image );
}
////////////////////////////////////////////////////////////////////////////
// Draw a wifi map
107 void map_draw_wifi( map_t * map, rtk_fig_t * fig, int index )
{
int i, j;
int level, col;
map_cell_t * cell;
uint16_t * image, * mask;
uint16_t * ipix, * mpix;
image = malloc( map->size_x * map->size_y * sizeof( image[0] ) );
mask = malloc( map->size_x * map->size_y * sizeof( mask[0] ) );
// Draw wifi levels
for ( j = 0; j < map->size_y; j++ ) {
for ( i = 0; i < map->size_x; i++ ) {
cell = map->cells + MAP_INDEX( map, i, j );
ipix = image + ( j * map->size_x + i );
mpix = mask + ( j * map->size_x + i );
level = cell->wifi_levels[index];
if ( cell->occ_state == -1 && level != 0 ) {
col = 255 * ( 100 + level ) / 100;
*ipix = RTK_RGB16( col, col, col );
*mpix = 1;
} else {
*mpix = 0;
}
}
}
// Draw the entire occupancy map as an image
rtk_fig_image(
fig, map->origin_x, map->origin_y, 0,
map->scale, map->size_x, map->size_y, 16, image, mask );
free( mask );
free( image );
}
#endif
1 /*
* Player - One Hell of a Robot Server
* Copyright ( C ) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Range routines
* Author: Andrew Howard
* Date: 18 Jan 2003
* CVS: $Id: map_range.c 1347 2003-05-05 06:24:33Z inspectorg $
**************************************************************************/
#include <assert.h>
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include "nav2_amcl/map/map.hpp"
// Extract a single range reading from the map. Unknown cells and/or
// out-of-bound cells are treated as occupied, which makes it easy to
// use Stage bitmap files.
38 double map_calc_range( map_t * map, double ox, double oy, double oa, double max_range )
{
// Bresenham raytracing
int x0, x1, y0, y1;
int x, y;
int xstep, ystep;
char steep;
int tmp;
int deltax, deltay, error, deltaerr;
x0 = MAP_GXWX( map, ox );
y0 = MAP_GYWY( map, oy );
x1 = MAP_GXWX( map, ox + max_range * cos( oa ) );
y1 = MAP_GYWY( map, oy + max_range * sin( oa ) );
if ( abs( y1 - y0 ) > abs( x1 - x0 ) ) {
steep = 1;
} else {
steep = 0;
}
if ( steep ) {
tmp = x0;
x0 = y0;
y0 = tmp;
tmp = x1;
x1 = y1;
y1 = tmp;
}
deltax = abs( x1 - x0 );
deltay = abs( y1 - y0 );
error = 0;
deltaerr = deltay;
x = x0;
y = y0;
if ( x0 < x1 ) {
xstep = 1;
} else {
xstep = -1;
}
if ( y0 < y1 ) {
ystep = 1;
} else {
ystep = -1;
}
if ( steep ) {
if ( !MAP_VALID( map, y, x ) || map->cells[MAP_INDEX( map, y, x )].occ_state > -1 ) {
return sqrt( ( x - x0 ) * ( x - x0 ) + ( y - y0 ) * ( y - y0 ) ) * map->scale;
}
} else {
if ( !MAP_VALID( map, x, y ) || map->cells[MAP_INDEX( map, x, y )].occ_state > -1 ) {
return sqrt( ( x - x0 ) * ( x - x0 ) + ( y - y0 ) * ( y - y0 ) ) * map->scale;
}
}
while ( x != ( x1 + xstep * 1 ) ) {
x += xstep;
error += deltaerr;
if ( 2 * error >= deltax ) {
y += ystep;
error -= deltax;
}
if ( steep ) {
if ( !MAP_VALID( map, y, x ) || map->cells[MAP_INDEX( map, y, x )].occ_state > -1 ) {
return sqrt( ( x - x0 ) * ( x - x0 ) + ( y - y0 ) * ( y - y0 ) ) * map->scale;
}
} else {
if ( !MAP_VALID( map, x, y ) || map->cells[MAP_INDEX( map, x, y )].occ_state > -1 ) {
return sqrt( ( x - x0 ) * ( x - x0 ) + ( y - y0 ) * ( y - y0 ) ) * map->scale;
}
}
}
return max_range;
}
1 /*
* Player - One Hell of a Robot Server
* Copyright ( C ) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
#include "nav2_amcl/motion_model/differential_motion_model.hpp"
namespace nav2_amcl
{
void
28 DifferentialMotionModel::initialize(
double alpha1, double alpha2, double alpha3, double alpha4,
double alpha5 )
{
alpha1_ = alpha1;
alpha2_ = alpha2;
alpha3_ = alpha3;
alpha4_ = alpha4;
alpha5_ = alpha5;
}
void
40 DifferentialMotionModel::odometryUpdate(
41 pf_t * pf, const pf_vector_t & pose,
42 const pf_vector_t & delta )
{
// Compute the new sample poses
pf_sample_set_t * set;
set = pf->sets + pf->current_set;
pf_vector_t old_pose = pf_vector_sub( pose, delta );
// Implement sample_motion_odometry ( Prob Rob p 136 )
double delta_rot1, delta_trans, delta_rot2;
double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
double delta_rot1_noise, delta_rot2_noise;
// Avoid computing a bearing from two poses that are extremely near each
// other ( happens on in-place rotation ).
if ( sqrt(
delta.v[1] * delta.v[1] +
delta.v[0] * delta.v[0] ) < 0.01 )
{
delta_rot1 = 0.0;
} else {
delta_rot1 = angleutils::angle_diff(
atan2( delta.v[1], delta.v[0] ),
old_pose.v[2] );
}
delta_trans = sqrt(
delta.v[0] * delta.v[0] +
delta.v[1] * delta.v[1] );
delta_rot2 = angleutils::angle_diff( delta.v[2], delta_rot1 );
// We want to treat backward and forward motion symmetrically for the
// noise model to be applied below. The standard model seems to assume
// forward motion.
delta_rot1_noise = std::min(
fabs( angleutils::angle_diff( delta_rot1, 0.0 ) ),
fabs( angleutils::angle_diff( delta_rot1, M_PI ) ) );
delta_rot2_noise = std::min(
fabs( angleutils::angle_diff( delta_rot2, 0.0 ) ),
fabs( angleutils::angle_diff( delta_rot2, M_PI ) ) );
for ( int i = 0; i < set->sample_count; i++ ) {
pf_sample_t * sample = set->samples + i;
// Sample pose differences
delta_rot1_hat = angleutils::angle_diff(
delta_rot1,
pf_ran_gaussian(
sqrt(
alpha1_ * delta_rot1_noise * delta_rot1_noise +
alpha2_ * delta_trans * delta_trans ) ) );
delta_trans_hat = delta_trans -
pf_ran_gaussian(
sqrt(
alpha3_ * delta_trans * delta_trans +
alpha4_ * delta_rot1_noise * delta_rot1_noise +
alpha4_ * delta_rot2_noise * delta_rot2_noise ) );
delta_rot2_hat = angleutils::angle_diff(
delta_rot2,
pf_ran_gaussian(
sqrt(
alpha1_ * delta_rot2_noise * delta_rot2_noise +
alpha2_ * delta_trans * delta_trans ) ) );
// Apply sampled update to particle pose
sample->pose.v[0] += delta_trans_hat *
cos( sample->pose.v[2] + delta_rot1_hat );
sample->pose.v[1] += delta_trans_hat *
sin( sample->pose.v[2] + delta_rot1_hat );
sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
}
}
} // namespace nav2_amcl
#include <pluginlib/class_list_macros.hpp>
117 PLUGINLIB_EXPORT_CLASS( nav2_amcl::DifferentialMotionModel, nav2_amcl::MotionModel )
1 /*
* Player - One Hell of a Robot Server
* Copyright ( C ) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
#include "nav2_amcl/motion_model/omni_motion_model.hpp"
namespace nav2_amcl
{
void
28 OmniMotionModel::initialize(
double alpha1, double alpha2, double alpha3, double alpha4,
double alpha5 )
{
alpha1_ = alpha1;
alpha2_ = alpha2;
alpha3_ = alpha3;
alpha4_ = alpha4;
alpha5_ = alpha5;
}
void
40 OmniMotionModel::odometryUpdate(
41 pf_t * pf, const pf_vector_t & pose,
42 const pf_vector_t & delta )
{
// Compute the new sample poses
pf_sample_set_t * set;
set = pf->sets + pf->current_set;
pf_vector_t old_pose = pf_vector_sub( pose, delta );
double delta_trans, delta_rot, delta_bearing;
double delta_trans_hat, delta_rot_hat, delta_strafe_hat;
delta_trans = sqrt(
delta.v[0] * delta.v[0] +
delta.v[1] * delta.v[1] );
delta_rot = delta.v[2];
// Precompute a couple of things
double trans_hat_stddev = sqrt(
alpha3_ * ( delta_trans * delta_trans ) +
alpha4_ * ( delta_rot * delta_rot ) );
double rot_hat_stddev = sqrt(
alpha1_ * ( delta_rot * delta_rot ) +
alpha2_ * ( delta_trans * delta_trans ) );
double strafe_hat_stddev = sqrt(
alpha4_ * ( delta_rot * delta_rot ) +
alpha5_ * ( delta_trans * delta_trans ) );
for ( int i = 0; i < set->sample_count; i++ ) {
pf_sample_t * sample = set->samples + i;
delta_bearing = angleutils::angle_diff(
atan2( delta.v[1], delta.v[0] ),
old_pose.v[2] ) + sample->pose.v[2];
double cs_bearing = cos( delta_bearing );
double sn_bearing = sin( delta_bearing );
// Sample pose differences
delta_trans_hat = delta_trans + pf_ran_gaussian( trans_hat_stddev );
delta_rot_hat = delta_rot + pf_ran_gaussian( rot_hat_stddev );
delta_strafe_hat = 0 + pf_ran_gaussian( strafe_hat_stddev );
// Apply sampled update to particle pose
sample->pose.v[0] += ( delta_trans_hat * cs_bearing +
delta_strafe_hat * sn_bearing );
sample->pose.v[1] += ( delta_trans_hat * sn_bearing -
delta_strafe_hat * cs_bearing );
sample->pose.v[2] += delta_rot_hat;
}
}
} // namespace nav2_amcl
#include <pluginlib/class_list_macros.hpp>
94 PLUGINLIB_EXPORT_CLASS( nav2_amcl::OmniMotionModel, nav2_amcl::MotionModel )
1 /*
* Player - One Hell of a Robot Server
* Copyright ( C ) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/* Eigen decomposition code for symmetric 3x3 matrices, copied from the public
domain Java Matrix library JAMA. */
#include <math.h>
#ifndef MAX
#define MAX( a, b ) ( ( a ) > ( b ) ? ( a ) : ( b ) )
#endif
#ifdef _MSC_VER
#define n 3
#else
static int n = 3;
#endif
// Symmetric Householder reduction to tridiagonal form.
38 static void tred2( double V[n][n], double d[n], double e[n] )
{
// This is derived from the Algol procedures tred2 by
// Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
// Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
// Fortran subroutine in EISPACK.
int i, j, k;
double f, g, h, hh;
for ( j = 0; j < n; j++ ) {
d[j] = V[n - 1][j];
}
// Householder reduction to tridiagonal form.
for ( i = n - 1; i > 0; i-- ) {
// Scale to avoid under/overflow.
double scale = 0.0;
double h = 0.0;
for ( k = 0; k < i; k++ ) {
scale = scale + fabs( d[k] );
}
if ( scale == 0.0 ) {
e[i] = d[i - 1];
for ( j = 0; j < i; j++ ) {
d[j] = V[i - 1][j];
V[i][j] = 0.0;
V[j][i] = 0.0;
}
} else {
// Generate Householder vector.
for ( k = 0; k < i; k++ ) {
d[k] /= scale;
h += d[k] * d[k];
}
f = d[i - 1];
g = sqrt( h );
if ( f > 0 ) {
g = -g;
}
e[i] = scale * g;
h = h - f * g;
d[i - 1] = f - g;
for ( j = 0; j < i; j++ ) {
e[j] = 0.0;
}
// Apply similarity transformation to remaining columns.
for ( j = 0; j < i; j++ ) {
f = d[j];
V[j][i] = f;
g = e[j] + V[j][j] * f;
for ( k = j + 1; k <= i - 1; k++ ) {
g += V[k][j] * d[k];
e[k] += V[k][j] * f;
}
e[j] = g;
}
f = 0.0;
for ( j = 0; j < i; j++ ) {
e[j] /= h;
f += e[j] * d[j];
}
hh = f / ( h + h );
for ( j = 0; j < i; j++ ) {
e[j] -= hh * d[j];
}
for ( j = 0; j < i; j++ ) {
f = d[j];
g = e[j];
for ( k = j; k <= i - 1; k++ ) {
V[k][j] -= ( f * e[k] + g * d[k] );
}
d[j] = V[i - 1][j];
V[i][j] = 0.0;
}
}
d[i] = h;
}
// Accumulate transformations.
for ( i = 0; i < n - 1; i++ ) {
V[n - 1][i] = V[i][i];
V[i][i] = 1.0;
h = d[i + 1];
if ( h != 0.0 ) {
for ( k = 0; k <= i; k++ ) {
d[k] = V[k][i + 1] / h;
}
for ( j = 0; j <= i; j++ ) {
g = 0.0;
for ( k = 0; k <= i; k++ ) {
g += V[k][i + 1] * V[k][j];
}
for ( k = 0; k <= i; k++ ) {
V[k][j] -= g * d[k];
}
}
}
for ( k = 0; k <= i; k++ ) {
V[k][i + 1] = 0.0;
}
}
for ( j = 0; j < n; j++ ) {
d[j] = V[n - 1][j];
V[n - 1][j] = 0.0;
}
V[n - 1][n - 1] = 1.0;
e[0] = 0.0;
}
// Symmetric tridiagonal QL algorithm.
154 static void tql2( double V[n][n], double d[n], double e[n] )
{
// This is derived from the Algol procedures tql2, by
// Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
// Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
// Fortran subroutine in EISPACK.
int i, j, m, l, k;
double g, p, r, dl1, h, f, tst1, eps;
double c, c2, c3, el1, s, s2;
for ( i = 1; i < n; i++ ) {
e[i - 1] = e[i];
}
e[n - 1] = 0.0;
f = 0.0;
tst1 = 0.0;
eps = pow( 2.0, -52.0 );
for ( l = 0; l < n; l++ ) {
// Find small subdiagonal element
tst1 = MAX( tst1, fabs( d[l] ) + fabs( e[l] ) );
m = l;
while ( m < n ) {
if ( fabs( e[m] ) <= eps * tst1 ) {
break;
}
m++;
}
// If m == l, d[l] is an eigenvalue,
// otherwise, iterate.
if ( m > l ) {
int iter = 0;
do {
iter = iter + 1; // ( Could check iteration count here. )
// Compute implicit shift
g = d[l];
p = ( d[l + 1] - g ) / ( 2.0 * e[l] );
r = hypot( p, 1.0 );
if ( p < 0 ) {
r = -r;
}
d[l] = e[l] / ( p + r );
d[l + 1] = e[l] * ( p + r );
dl1 = d[l + 1];
h = g - d[l];
for ( i = l + 2; i < n; i++ ) {
d[i] -= h;
}
f = f + h;
// Implicit QL transformation.
p = d[m];
c = 1.0;
c2 = c;
c3 = c;
el1 = e[l + 1];
s = 0.0;
s2 = 0.0;
for ( i = m - 1; i >= l; i-- ) {
c3 = c2;
c2 = c;
s2 = s;
g = c * e[i];
h = c * p;
r = hypot( p, e[i] );
e[i + 1] = s * r;
s = e[i] / r;
c = p / r;
p = c * d[i] - s * g;
d[i + 1] = h + s * ( c * g + s * d[i] );
// Accumulate transformation.
for ( k = 0; k < n; k++ ) {
h = V[k][i + 1];
V[k][i + 1] = s * V[k][i] + c * h;
V[k][i] = c * V[k][i] - s * h;
}
}
p = -s * s2 * c3 * el1 * e[l] / dl1;
e[l] = s * p;
d[l] = c * p;
// Check for convergence.
} while ( fabs( e[l] ) > eps * tst1 );
}
d[l] = d[l] + f;
e[l] = 0.0;
}
// Sort eigenvalues and corresponding vectors.
for ( i = 0; i < n - 1; i++ ) {
k = i;
p = d[i];
for ( j = i + 1; j < n; j++ ) {
if ( d[j] < p ) {
k = j;
p = d[j];
}
}
if ( k != i ) {
d[k] = d[i];
d[i] = p;
for ( j = 0; j < n; j++ ) {
p = V[j][i];
V[j][i] = V[j][k];
V[j][k] = p;
}
}
}
}
271 void eigen_decomposition( double A[n][n], double V[n][n], double d[n] )
{
int i, j;
double e[n]; // NOLINT
for ( i = 0; i < n; i++ ) {
for ( j = 0; j < n; j++ ) {
V[i][j] = A[i][j];
}
}
tred2( V, d, e );
tql2( V, d, e );
}
1 /*
* Player - One Hell of a Robot Server
* Copyright ( C ) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Simple particle filter for localization.
* Author: Andrew Howard
* Date: 10 Dec 2002
* CVS: $Id: pf.c 6345 2008-04-17 01:36:39Z gerkey $
*************************************************************************/
#include <float.h>
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <time.h>
#include "nav2_amcl/pf/pf.hpp"
#include "nav2_amcl/pf/pf_pdf.hpp"
#include "nav2_amcl/pf/pf_kdtree.hpp"
#include "nav2_amcl/portable_utils.hpp"
// Compute the required number of samples, given that there are k bins
// with samples in them.
43 static int pf_resample_limit( pf_t * pf, int k );
// Create a new filter
47 pf_t * pf_alloc(
int min_samples, int max_samples,
double alpha_slow, double alpha_fast,
pf_init_model_fn_t random_pose_fn, void * random_pose_data )
{
int i, j;
pf_t * pf;
pf_sample_set_t * set;
pf_sample_t * sample;
srand48( time( NULL ) );
pf = calloc( 1, sizeof( pf_t ) );
pf->random_pose_fn = random_pose_fn;
pf->random_pose_data = random_pose_data;
pf->min_samples = min_samples;
pf->max_samples = max_samples;
// Control parameters for the population size calculation. [err] is
// the max error between the true distribution and the estimated
// distribution. [z] is the upper standard normal quantile for ( 1 -
// p ), where p is the probability that the error on the estimated
// distrubition will be less than [err].
pf->pop_err = 0.01;
pf->pop_z = 3;
pf->dist_threshold = 0.5;
pf->current_set = 0;
for ( j = 0; j < 2; j++ ) {
set = pf->sets + j;
set->sample_count = max_samples;
set->samples = calloc( max_samples, sizeof( pf_sample_t ) );
for ( i = 0; i < set->sample_count; i++ ) {
sample = set->samples + i;
sample->pose.v[0] = 0.0;
sample->pose.v[1] = 0.0;
sample->pose.v[2] = 0.0;
sample->weight = 1.0 / max_samples;
}
// HACK: is 3 times max_samples enough?
set->kdtree = pf_kdtree_alloc( 3 * max_samples );
set->cluster_count = 0;
set->cluster_max_count = max_samples;
set->clusters = calloc( set->cluster_max_count, sizeof( pf_cluster_t ) );
set->mean = pf_vector_zero( );
set->cov = pf_matrix_zero( );
}
pf->w_slow = 0.0;
pf->w_fast = 0.0;
pf->alpha_slow = alpha_slow;
pf->alpha_fast = alpha_fast;
// set converged to 0
pf_init_converged( pf );
return pf;
}
// Free an existing filter
115 void pf_free( pf_t * pf )
{
int i;
for ( i = 0; i < 2; i++ ) {
free( pf->sets[i].clusters );
pf_kdtree_free( pf->sets[i].kdtree );
free( pf->sets[i].samples );
}
free( pf );
}
// Initialize the filter using a guassian
128 void pf_init( pf_t * pf, pf_vector_t mean, pf_matrix_t cov )
{
int i;
pf_sample_set_t * set;
pf_sample_t * sample;
pf_pdf_gaussian_t * pdf;
set = pf->sets + pf->current_set;
// Create the kd tree for adaptive sampling
pf_kdtree_clear( set->kdtree );
set->sample_count = pf->max_samples;
pdf = pf_pdf_gaussian_alloc( mean, cov );
// Compute the new sample poses
for ( i = 0; i < set->sample_count; i++ ) {
sample = set->samples + i;
sample->weight = 1.0 / pf->max_samples;
sample->pose = pf_pdf_gaussian_sample( pdf );
// Add sample to histogram
pf_kdtree_insert( set->kdtree, sample->pose, sample->weight );
}
pf->w_slow = pf->w_fast = 0.0;
pf_pdf_gaussian_free( pdf );
// Re-compute cluster statistics
pf_cluster_stats( pf, set );
// set converged to 0
pf_init_converged( pf );
}
// Initialize the filter using some model
167 void pf_init_model( pf_t * pf, pf_init_model_fn_t init_fn, void * init_data )
{
int i;
pf_sample_set_t * set;
pf_sample_t * sample;
set = pf->sets + pf->current_set;
// Create the kd tree for adaptive sampling
pf_kdtree_clear( set->kdtree );
set->sample_count = pf->max_samples;
// Compute the new sample poses
for ( i = 0; i < set->sample_count; i++ ) {
sample = set->samples + i;
sample->weight = 1.0 / pf->max_samples;
sample->pose = ( *init_fn )( init_data );
// Add sample to histogram
pf_kdtree_insert( set->kdtree, sample->pose, sample->weight );
}
pf->w_slow = pf->w_fast = 0.0;
// Re-compute cluster statistics
pf_cluster_stats( pf, set );
// set converged to 0
pf_init_converged( pf );
}
199 void pf_init_converged( pf_t * pf )
{
pf_sample_set_t * set;
set = pf->sets + pf->current_set;
set->converged = 0;
pf->converged = 0;
}
207 int pf_update_converged( pf_t * pf )
{
int i;
pf_sample_set_t * set;
pf_sample_t * sample;
set = pf->sets + pf->current_set;
double mean_x = 0, mean_y = 0;
for ( i = 0; i < set->sample_count; i++ ) {
sample = set->samples + i;
mean_x += sample->pose.v[0];
mean_y += sample->pose.v[1];
}
mean_x /= set->sample_count;
mean_y /= set->sample_count;
for ( i = 0; i < set->sample_count; i++ ) {
sample = set->samples + i;
if ( fabs( sample->pose.v[0] - mean_x ) > pf->dist_threshold ||
fabs( sample->pose.v[1] - mean_y ) > pf->dist_threshold )
{
set->converged = 0;
pf->converged = 0;
return 0;
}
}
set->converged = 1;
pf->converged = 1;
return 1;
}
// Update the filter with some new action
// void pf_update_action( pf_t * pf, pf_action_model_fn_t action_fn, void * action_data )
// {
// pf_sample_set_t * set;
// set = pf->sets + pf->current_set;
// ( *action_fn )( action_data, set );
// }
// Update the filter with some new sensor observation
251 void pf_update_sensor( pf_t * pf, pf_sensor_model_fn_t sensor_fn, void * sensor_data )
{
int i;
pf_sample_set_t * set;
pf_sample_t * sample;
double total;
set = pf->sets + pf->current_set;
// Compute the sample weights
total = ( *sensor_fn )( sensor_data, set );
if ( total > 0.0 ) {
// Normalize weights
double w_avg = 0.0;
for ( i = 0; i < set->sample_count; i++ ) {
sample = set->samples + i;
w_avg += sample->weight;
sample->weight /= total;
}
// Update running averages of likelihood of samples ( Prob Rob p258 )
w_avg /= set->sample_count;
if ( pf->w_slow == 0.0 ) {
pf->w_slow = w_avg;
} else {
pf->w_slow += pf->alpha_slow * ( w_avg - pf->w_slow );
}
if ( pf->w_fast == 0.0 ) {
pf->w_fast = w_avg;
} else {
pf->w_fast += pf->alpha_fast * ( w_avg - pf->w_fast );
}
} else {
// Handle zero total
for ( i = 0; i < set->sample_count; i++ ) {
sample = set->samples + i;
sample->weight = 1.0 / set->sample_count;
}
}
}
// Resample the distribution
294 void pf_update_resample( pf_t * pf )
{
int i;
double total;
pf_sample_set_t * set_a, * set_b;
pf_sample_t * sample_a, * sample_b;
// double r, c, U;
// int m;
// double count_inv;
double * c;
double w_diff;
set_a = pf->sets + pf->current_set;
set_b = pf->sets + ( pf->current_set + 1 ) % 2;
// Build up cumulative probability table for resampling.
// TODO( ? ): Replace this with a more efficient procedure
// ( e.g., http://www.network-theory.co.uk/docs/gslref/GeneralDiscreteDistributions.html )
c = ( double * )malloc( sizeof( double ) * ( set_a->sample_count + 1 ) );
c[0] = 0.0;
for ( i = 0; i < set_a->sample_count; i++ ) {
c[i + 1] = c[i] + set_a->samples[i].weight;
}
// Create the kd tree for adaptive sampling
pf_kdtree_clear( set_b->kdtree );
// Draw samples from set a to create set b.
total = 0;
set_b->sample_count = 0;
w_diff = 1.0 - pf->w_fast / pf->w_slow;
if ( w_diff < 0.0 ) {
w_diff = 0.0;
}
// printf( "w_diff: %9.6f\n", w_diff );
// Can't ( easily ) combine low-variance sampler with KLD adaptive
// sampling, so we'll take the more traditional route.
/*
// Low-variance resampler, taken from Probabilistic Robotics, p110
count_inv = 1.0/set_a->sample_count;
r = drand48( ) * count_inv;
c = set_a->samples[0].weight;
i = 0;
m = 0;
*/
while ( set_b->sample_count < pf->max_samples ) {
sample_b = set_b->samples + set_b->sample_count++;
if ( drand48( ) < w_diff ) {
sample_b->pose = ( pf->random_pose_fn )( pf->random_pose_data );
} else {
// Can't ( easily ) combine low-variance sampler with KLD adaptive
// sampling, so we'll take the more traditional route.
/*
// Low-variance resampler, taken from Probabilistic Robotics, p110
U = r + m * count_inv;
while( U>c )
{
i++;
// Handle wrap-around by resetting counters and picking a new random
// number
if( i >= set_a->sample_count )
{
r = drand48( ) * count_inv;
c = set_a->samples[0].weight;
i = 0;
m = 0;
U = r + m * count_inv;
continue;
}
c += set_a->samples[i].weight;
}
m++;
*/
// Naive discrete event sampler
double r;
r = drand48( );
for ( i = 0; i < set_a->sample_count; i++ ) {
if ( ( c[i] <= r ) && ( r < c[i + 1] ) ) {
break;
}
}
assert( i < set_a->sample_count );
sample_a = set_a->samples + i;
assert( sample_a->weight > 0 );
// Add sample to list
sample_b->pose = sample_a->pose;
}
sample_b->weight = 1.0;
total += sample_b->weight;
// Add sample to histogram
pf_kdtree_insert( set_b->kdtree, sample_b->pose, sample_b->weight );
// See if we have enough samples yet
if ( set_b->sample_count > pf_resample_limit( pf, set_b->kdtree->leaf_count ) ) {
break;
}
}
// Reset averages, to avoid spiraling off into complete randomness.
if ( w_diff > 0.0 ) {
pf->w_slow = pf->w_fast = 0.0;
}
// fprintf( stderr, "\n\n" );
// Normalize weights
for ( i = 0; i < set_b->sample_count; i++ ) {
sample_b = set_b->samples + i;
sample_b->weight /= total;
}
// Re-compute cluster statistics
pf_cluster_stats( pf, set_b );
// Use the newly created sample set
pf->current_set = ( pf->current_set + 1 ) % 2;
pf_update_converged( pf );
free( c );
}
// Compute the required number of samples, given that there are k bins
// with samples in them. This is taken directly from Fox et al.
430 int pf_resample_limit( pf_t * pf, int k )
{
double a, b, c, x;
int n;
if ( k <= 1 ) {
return pf->max_samples;
}
a = 1;
b = 2 / ( 9 * ( ( double ) k - 1 ) );
c = sqrt( 2 / ( 9 * ( ( double ) k - 1 ) ) ) * pf->pop_z;
x = a - b + c;
n = ( int ) ceil( ( k - 1 ) / ( 2 * pf->pop_err ) * x * x * x );
if ( n < pf->min_samples ) {
return pf->min_samples;
}
if ( n > pf->max_samples ) {
return pf->max_samples;
}
return n;
}
// Re-compute the cluster statistics for a sample set
458 void pf_cluster_stats( pf_t * pf, pf_sample_set_t * set )
{
( void )pf;
int i, j, k, cidx;
pf_sample_t * sample;
pf_cluster_t * cluster;
// Workspace
double m[4], c[2][2];
size_t count;
double weight;
// Cluster the samples
pf_kdtree_cluster( set->kdtree );
// Initialize cluster stats
set->cluster_count = 0;
for ( i = 0; i < set->cluster_max_count; i++ ) {
cluster = set->clusters + i;
cluster->count = 0;
cluster->weight = 0;
cluster->mean = pf_vector_zero( );
cluster->cov = pf_matrix_zero( );
for ( j = 0; j < 4; j++ ) {
cluster->m[j] = 0.0;
}
for ( j = 0; j < 2; j++ ) {
for ( k = 0; k < 2; k++ ) {
cluster->c[j][k] = 0.0;
}
}
}
// Initialize overall filter stats
count = 0;
weight = 0.0;
set->mean = pf_vector_zero( );
set->cov = pf_matrix_zero( );
for ( j = 0; j < 4; j++ ) {
m[j] = 0.0;
}
for ( j = 0; j < 2; j++ ) {
for ( k = 0; k < 2; k++ ) {
c[j][k] = 0.0;
}
}
// Compute cluster stats
for ( i = 0; i < set->sample_count; i++ ) {
sample = set->samples + i;
// printf( "%d %f %f %f\n", i, sample->pose.v[0], sample->pose.v[1], sample->pose.v[2] );
// Get the cluster label for this sample
cidx = pf_kdtree_get_cluster( set->kdtree, sample->pose );
assert( cidx >= 0 );
if ( cidx >= set->cluster_max_count ) {
continue;
}
if ( cidx + 1 > set->cluster_count ) {
set->cluster_count = cidx + 1;
}
cluster = set->clusters + cidx;
cluster->count += 1;
cluster->weight += sample->weight;
count += 1;
weight += sample->weight;
// Compute mean
cluster->m[0] += sample->weight * sample->pose.v[0];
cluster->m[1] += sample->weight * sample->pose.v[1];
cluster->m[2] += sample->weight * cos( sample->pose.v[2] );
cluster->m[3] += sample->weight * sin( sample->pose.v[2] );
m[0] += sample->weight * sample->pose.v[0];
m[1] += sample->weight * sample->pose.v[1];
m[2] += sample->weight * cos( sample->pose.v[2] );
m[3] += sample->weight * sin( sample->pose.v[2] );
// Compute covariance in linear components
for ( j = 0; j < 2; j++ ) {
for ( k = 0; k < 2; k++ ) {
cluster->c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
}
}
}
// Normalize
for ( i = 0; i < set->cluster_count; i++ ) {
cluster = set->clusters + i;
cluster->mean.v[0] = cluster->m[0] / cluster->weight;
cluster->mean.v[1] = cluster->m[1] / cluster->weight;
cluster->mean.v[2] = atan2( cluster->m[3], cluster->m[2] );
cluster->cov = pf_matrix_zero( );
// Covariance in linear components
for ( j = 0; j < 2; j++ ) {
for ( k = 0; k < 2; k++ ) {
cluster->cov.m[j][k] = cluster->c[j][k] / cluster->weight -
cluster->mean.v[j] * cluster->mean.v[k];
}
}
// Covariance in angular components; I think this is the correct
// formula for circular statistics.
cluster->cov.m[2][2] = -2 * log(
sqrt(
cluster->m[2] * cluster->m[2] +
cluster->m[3] * cluster->m[3] ) );
// printf( "cluster %d %d %f ( %f %f %f )\n", i, cluster->count, cluster->weight,
// cluster->mean.v[0], cluster->mean.v[1], cluster->mean.v[2] );
// pf_matrix_fprintf( cluster->cov, stdout, "%e" );
}
// Compute overall filter stats
set->mean.v[0] = m[0] / weight;
set->mean.v[1] = m[1] / weight;
set->mean.v[2] = atan2( m[3], m[2] );
// Covariance in linear components
for ( j = 0; j < 2; j++ ) {
for ( k = 0; k < 2; k++ ) {
set->cov.m[j][k] = c[j][k] / weight - set->mean.v[j] * set->mean.v[k];
}
}
// Covariance in angular components; I think this is the correct
// formula for circular statistics.
set->cov.m[2][2] = -2 * log( sqrt( m[2] * m[2] + m[3] * m[3] ) );
}
// Compute the CEP statistics ( mean and variance ).
// void pf_get_cep_stats( pf_t * pf, pf_vector_t * mean, double * var )
// {
// int i;
// double mn, mx, my, mrr;
// pf_sample_set_t * set;
// pf_sample_t * sample;
// set = pf->sets + pf->current_set;
// mn = 0.0;
// mx = 0.0;
// my = 0.0;
// mrr = 0.0;
// for ( i = 0; i < set->sample_count; i++ ) {
// sample = set->samples + i;
// mn += sample->weight;
// mx += sample->weight * sample->pose.v[0];
// my += sample->weight * sample->pose.v[1];
// mrr += sample->weight * sample->pose.v[0] * sample->pose.v[0];
// mrr += sample->weight * sample->pose.v[1] * sample->pose.v[1];
// }
// mean->v[0] = mx / mn;
// mean->v[1] = my / mn;
// mean->v[2] = 0.0;
// *var = mrr / mn - ( mx * mx / ( mn * mn ) + my * my / ( mn * mn ) );
// }
// Get the statistics for a particular cluster.
633 int pf_get_cluster_stats(
pf_t * pf, int clabel, double * weight,
pf_vector_t * mean, pf_matrix_t * cov )
{
pf_sample_set_t * set;
pf_cluster_t * cluster;
set = pf->sets + pf->current_set;
if ( clabel >= set->cluster_count ) {
return 0;
}
cluster = set->clusters + clabel;
*weight = cluster->weight;
*mean = cluster->mean;
*cov = cluster->cov;
return 1;
}
1 /*
* Player - One Hell of a Robot Server
* Copyright ( C ) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Particle filter; drawing routines
* Author: Andrew Howard
* Date: 10 Dec 2002
* CVS: $Id: pf_draw.c 7057 2008-10-02 00:44:06Z gbiggs $
*************************************************************************/
#pragma GCC diagnostic ignored "-Wpedantic"
#ifdef INCLUDE_RTKGUI
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <rtk.h>
#include "nav2_amcl/pf/pf.hpp"
#include "nav2_amcl/pf/pf_pdf.hpp"
#include "nav2_amcl/pf/pf_kdtree.hpp"
// Draw the statistics
43 void pf_draw_statistics( pf_t * pf, rtk_fig_t * fig );
// Draw the sample set
47 void pf_draw_samples( pf_t * pf, rtk_fig_t * fig, int max_samples )
{
int i;
double px, py, pa;
pf_sample_set_t * set;
pf_sample_t * sample;
set = pf->sets + pf->current_set;
max_samples = MIN( max_samples, set->sample_count );
for ( i = 0; i < max_samples; i++ ) {
sample = set->samples + i;
px = sample->pose.v[0];
py = sample->pose.v[1];
pa = sample->pose.v[2];
// printf( "%f %f\n", px, py );
rtk_fig_point( fig, px, py );
rtk_fig_arrow( fig, px, py, pa, 0.1, 0.02 );
// rtk_fig_rectangle( fig, px, py, 0, 0.1, 0.1, 0 );
}
}
// Draw the hitogram ( kd tree )
74 void pf_draw_hist( pf_t * pf, rtk_fig_t * fig )
{
pf_sample_set_t * set;
set = pf->sets + pf->current_set;
rtk_fig_color( fig, 0.0, 0.0, 1.0 );
pf_kdtree_draw( set->kdtree, fig );
}
// Draw the CEP statistics
// void pf_draw_cep_stats( pf_t * pf, rtk_fig_t * fig )
// {
// pf_vector_t mean;
// double var;
// pf_get_cep_stats( pf, &mean, &var );
// var = sqrt( var );
// rtk_fig_color( fig, 0, 0, 1 );
// rtk_fig_ellipse( fig, mean.v[0], mean.v[1], mean.v[2], 3 * var, 3 * var, 0 );
// }
// Draw the cluster statistics
99 void pf_draw_cluster_stats( pf_t * pf, rtk_fig_t * fig )
{
int i;
pf_cluster_t * cluster;
pf_sample_set_t * set;
pf_vector_t mean;
pf_matrix_t cov;
pf_matrix_t r, d;
double weight, o, d1, d2;
set = pf->sets + pf->current_set;
for ( i = 0; i < set->cluster_count; i++ ) {
cluster = set->clusters + i;
weight = cluster->weight;
mean = cluster->mean;
cov = cluster->cov;
// Compute unitary representation S = R D R^T
pf_matrix_unitary( &r, &d, cov );
/* Debugging
printf( "mean = \n" );
pf_vector_fprintf( mean, stdout, "%e" );
printf( "cov = \n" );
pf_matrix_fprintf( cov, stdout, "%e" );
printf( "r = \n" );
pf_matrix_fprintf( r, stdout, "%e" );
printf( "d = \n" );
pf_matrix_fprintf( d, stdout, "%e" );
*/
// Compute the orientation of the error ellipse ( first eigenvector )
o = atan2( r.m[1][0], r.m[0][0] );
d1 = 6 * sqrt( d.m[0][0] );
d2 = 6 * sqrt( d.m[1][1] );
if ( d1 > 1e-3 && d2 > 1e-3 ) {
// Draw the error ellipse
rtk_fig_ellipse( fig, mean.v[0], mean.v[1], o, d1, d2, 0 );
rtk_fig_line_ex( fig, mean.v[0], mean.v[1], o, d1 );
rtk_fig_line_ex( fig, mean.v[0], mean.v[1], o + M_PI / 2, d2 );
}
// Draw a direction indicator
rtk_fig_arrow( fig, mean.v[0], mean.v[1], mean.v[2], 0.50, 0.10 );
rtk_fig_arrow( fig, mean.v[0], mean.v[1], mean.v[2] + 3 * sqrt( cov.m[2][2] ), 0.50, 0.10 );
rtk_fig_arrow( fig, mean.v[0], mean.v[1], mean.v[2] - 3 * sqrt( cov.m[2][2] ), 0.50, 0.10 );
}
}
#endif
1 /*
* Player - One Hell of a Robot Server
* Copyright ( C ) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: kd-tree functions
* Author: Andrew Howard
* Date: 18 Dec 2002
* CVS: $Id: pf_kdtree.c 7057 2008-10-02 00:44:06Z gbiggs $
*************************************************************************/
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "nav2_amcl/pf/pf_vector.hpp"
#include "nav2_amcl/pf/pf_kdtree.hpp"
// Compare keys to see if they are equal
39 static int pf_kdtree_equal( pf_kdtree_t * self, int key_a[], int key_b[] );
// Insert a node into the tree
42 static pf_kdtree_node_t * pf_kdtree_insert_node(
pf_kdtree_t * self, pf_kdtree_node_t * parent,
pf_kdtree_node_t * node, int key[], double value );
// Recursive node search
47 static pf_kdtree_node_t * pf_kdtree_find_node(
pf_kdtree_t * self, pf_kdtree_node_t * node,
int key[] );
// Recursively label nodes in this cluster
52 static void pf_kdtree_cluster_node( pf_kdtree_t * self, pf_kdtree_node_t * node, int depth );
// Recursive node printing
// static void pf_kdtree_print_node( pf_kdtree_t *self, pf_kdtree_node_t *node );
#ifdef INCLUDE_RTKGUI
// Recursively draw nodes
61 static void pf_kdtree_draw_node( pf_kdtree_t * self, pf_kdtree_node_t * node, rtk_fig_t * fig );
#endif
////////////////////////////////////////////////////////////////////////////////
// Create a tree
68 pf_kdtree_t * pf_kdtree_alloc( int max_size )
{
pf_kdtree_t * self;
self = calloc( 1, sizeof( pf_kdtree_t ) );
self->size[0] = 0.50;
self->size[1] = 0.50;
self->size[2] = ( 10 * M_PI / 180 );
self->root = NULL;
self->node_count = 0;
self->node_max_count = max_size;
self->nodes = calloc( self->node_max_count, sizeof( pf_kdtree_node_t ) );
self->leaf_count = 0;
return self;
}
////////////////////////////////////////////////////////////////////////////////
// Destroy a tree
92 void pf_kdtree_free( pf_kdtree_t * self )
{
free( self->nodes );
free( self );
}
////////////////////////////////////////////////////////////////////////////////
// Clear all entries from the tree
101 void pf_kdtree_clear( pf_kdtree_t * self )
{
self->root = NULL;
self->leaf_count = 0;
self->node_count = 0;
}
////////////////////////////////////////////////////////////////////////////////
// Insert a pose into the tree.
111 void pf_kdtree_insert( pf_kdtree_t * self, pf_vector_t pose, double value )
{
int key[3];
key[0] = floor( pose.v[0] / self->size[0] );
key[1] = floor( pose.v[1] / self->size[1] );
key[2] = floor( pose.v[2] / self->size[2] );
self->root = pf_kdtree_insert_node( self, NULL, self->root, key, value );
// Test code
/*
printf( "find %d %d %d\n", key[0], key[1], key[2] );
assert( pf_kdtree_find_node( self, self->root, key ) != NULL );
pf_kdtree_print_node( self, self->root );
printf( "\n" );
for ( i = 0; i < self->node_count; i++ )
{
node = self->nodes + i;
if ( node->leaf )
{
printf( "find %d %d %d\n", node->key[0], node->key[1], node->key[2] );
assert( pf_kdtree_find_node( self, self->root, node->key ) == node );
}
}
printf( "\n\n" );
*/
}
////////////////////////////////////////////////////////////////////////////////
// Determine the probability estimate for the given pose. TODO: this
// should do a kernel density estimate rather than a simple histogram.
// double pf_kdtree_get_prob( pf_kdtree_t * self, pf_vector_t pose )
// {
// int key[3];
// pf_kdtree_node_t * node;
// key[0] = floor( pose.v[0] / self->size[0] );
// key[1] = floor( pose.v[1] / self->size[1] );
// key[2] = floor( pose.v[2] / self->size[2] );
// node = pf_kdtree_find_node( self, self->root, key );
// if ( node == NULL ) {
// return 0.0;
// }
// return node->value;
// }
////////////////////////////////////////////////////////////////////////////////
// Determine the cluster label for the given pose
166 int pf_kdtree_get_cluster( pf_kdtree_t * self, pf_vector_t pose )
{
int key[3];
pf_kdtree_node_t * node;
key[0] = floor( pose.v[0] / self->size[0] );
key[1] = floor( pose.v[1] / self->size[1] );
key[2] = floor( pose.v[2] / self->size[2] );
node = pf_kdtree_find_node( self, self->root, key );
if ( node == NULL ) {
return -1;
}
return node->cluster;
}
////////////////////////////////////////////////////////////////////////////////
// Compare keys to see if they are equal
185 int pf_kdtree_equal( pf_kdtree_t * self, int key_a[], int key_b[] )
{
( void )self;
// double a, b;
if ( key_a[0] != key_b[0] ) {
return 0;
}
if ( key_a[1] != key_b[1] ) {
return 0;
}
if ( key_a[2] != key_b[2] ) {
return 0;
}
/* TODO: make this work ( pivot selection needs fixing, too )
// Normalize angles
a = key_a[2] * self->size[2];
a = atan2( sin( a ), cos( a ) ) / self->size[2];
b = key_b[2] * self->size[2];
b = atan2( sin( b ), cos( b ) ) / self->size[2];
if ( ( int ) a != ( int ) b )
return 0;
*/
return 1;
}
////////////////////////////////////////////////////////////////////////////////
// Insert a node into the tree
218 pf_kdtree_node_t * pf_kdtree_insert_node(
pf_kdtree_t * self, pf_kdtree_node_t * parent,
pf_kdtree_node_t * node, int key[], double value )
{
int i;
int split, max_split;
// If the node doesnt exist yet...
if ( node == NULL ) {
assert( self->node_count < self->node_max_count );
node = self->nodes + self->node_count++;
memset( node, 0, sizeof( pf_kdtree_node_t ) );
node->leaf = 1;
if ( parent == NULL ) {
node->depth = 0;
} else {
node->depth = parent->depth + 1;
}
for ( i = 0; i < 3; i++ ) {
node->key[i] = key[i];
}
node->value = value;
self->leaf_count += 1;
} else if ( node->leaf ) { // If the node exists, and it is a leaf node...
// If the keys are equal, increment the value
if ( pf_kdtree_equal( self, key, node->key ) ) {
node->value += value;
} else { // The keys are not equal, so split this node
// Find the dimension with the largest variance and do a mean
// split
max_split = 0;
node->pivot_dim = -1;
for ( i = 0; i < 3; i++ ) {
split = abs( key[i] - node->key[i] );
if ( split > max_split ) {
max_split = split;
node->pivot_dim = i;
}
}
assert( node->pivot_dim >= 0 );
node->pivot_value = ( key[node->pivot_dim] + node->key[node->pivot_dim] ) / 2.0;
if ( key[node->pivot_dim] < node->pivot_value ) {
node->children[0] = pf_kdtree_insert_node( self, node, NULL, key, value );
node->children[1] = pf_kdtree_insert_node( self, node, NULL, node->key, node->value );
} else {
node->children[0] = pf_kdtree_insert_node( self, node, NULL, node->key, node->value );
node->children[1] = pf_kdtree_insert_node( self, node, NULL, key, value );
}
node->leaf = 0;
self->leaf_count -= 1;
}
} else { // If the node exists, and it has children...
assert( node->children[0] != NULL );
assert( node->children[1] != NULL );
if ( key[node->pivot_dim] < node->pivot_value ) {
pf_kdtree_insert_node( self, node, node->children[0], key, value );
} else {
pf_kdtree_insert_node( self, node, node->children[1], key, value );
}
}
return node;
}
////////////////////////////////////////////////////////////////////////////////
// Recursive node search
293 pf_kdtree_node_t * pf_kdtree_find_node( pf_kdtree_t * self, pf_kdtree_node_t * node, int key[] )
{
if ( node->leaf ) {
// printf( "find : leaf %p %d %d %d\n", node, node->key[0], node->key[1], node->key[2] );
// If the keys are the same...
if ( pf_kdtree_equal( self, key, node->key ) ) {
return node;
} else {
return NULL;
}
} else {
// printf( "find : brch %p %d %f\n", node, node->pivot_dim, node->pivot_value );
assert( node->children[0] != NULL );
assert( node->children[1] != NULL );
// If the keys are different...
if ( key[node->pivot_dim] < node->pivot_value ) {
return pf_kdtree_find_node( self, node->children[0], key );
} else {
return pf_kdtree_find_node( self, node->children[1], key );
}
}
return NULL;
}
////////////////////////////////////////////////////////////////////////////////
// Recursive node printing
/*
void pf_kdtree_print_node( pf_kdtree_t *self, pf_kdtree_node_t *node )
{
if ( node->leaf )
{
printf( "( %+02d %+02d %+02d )\n", node->key[0], node->key[1], node->key[2] );
printf( "%*s", node->depth * 11, "" );
}
else
{
printf( "( %+02d %+02d %+02d ) ", node->key[0], node->key[1], node->key[2] );
pf_kdtree_print_node( self, node->children[0] );
pf_kdtree_print_node( self, node->children[1] );
}
return;
}
*/
////////////////////////////////////////////////////////////////////////////////
// Cluster the leaves in the tree
345 void pf_kdtree_cluster( pf_kdtree_t * self )
{
int i;
int queue_count, cluster_count;
pf_kdtree_node_t ** queue, * node;
queue_count = 0;
queue = calloc( self->node_count, sizeof( queue[0] ) );
// Put all the leaves in a queue
for ( i = 0; i < self->node_count; i++ ) {
node = self->nodes + i;
if ( node->leaf ) {
node->cluster = -1;
assert( queue_count < self->node_count );
queue[queue_count++] = node;
// TESTING; remove
assert( node == pf_kdtree_find_node( self, self->root, node->key ) );
}
}
cluster_count = 0;
// Do connected components for each node
while ( queue_count > 0 ) {
node = queue[--queue_count];
// If this node has already been labelled, skip it
if ( node->cluster >= 0 ) {
continue;
}
// Assign a label to this cluster
node->cluster = cluster_count++;
// Recursively label nodes in this cluster
pf_kdtree_cluster_node( self, node, 0 );
}
free( queue );
}
////////////////////////////////////////////////////////////////////////////////
// Recursively label nodes in this cluster
391 void pf_kdtree_cluster_node( pf_kdtree_t * self, pf_kdtree_node_t * node, int depth )
{
int i;
int nkey[3];
pf_kdtree_node_t * nnode;
for ( i = 0; i < 3 * 3 * 3; i++ ) {
nkey[0] = node->key[0] + ( i / 9 ) - 1;
nkey[1] = node->key[1] + ( ( i % 9 ) / 3 ) - 1;
nkey[2] = node->key[2] + ( ( i % 9 ) % 3 ) - 1;
nnode = pf_kdtree_find_node( self, self->root, nkey );
if ( nnode == NULL ) {
continue;
}
assert( nnode->leaf );
// This node already has a label; skip it. The label should be
// consistent, however.
if ( nnode->cluster >= 0 ) {
assert( nnode->cluster == node->cluster );
continue;
}
// Label this node and recurse
nnode->cluster = node->cluster;
pf_kdtree_cluster_node( self, nnode, depth + 1 );
}
}
#ifdef INCLUDE_RTKGUI
////////////////////////////////////////////////////////////////////////////////
// Draw the tree
428 void pf_kdtree_draw( pf_kdtree_t * self, rtk_fig_t * fig )
{
if ( self->root != NULL ) {
pf_kdtree_draw_node( self, self->root, fig );
}
}
////////////////////////////////////////////////////////////////////////////////
// Recursively draw nodes
438 void pf_kdtree_draw_node( pf_kdtree_t * self, pf_kdtree_node_t * node, rtk_fig_t * fig )
{
double ox, oy;
char text[64];
if ( node->leaf ) {
ox = ( node->key[0] + 0.5 ) * self->size[0];
oy = ( node->key[1] + 0.5 ) * self->size[1];
rtk_fig_rectangle( fig, ox, oy, 0.0, self->size[0], self->size[1], 0 );
// snprintf( text, sizeof( text ), "%0.3f", node->value );
// rtk_fig_text( fig, ox, oy, 0.0, text );
snprintf( text, sizeof( text ), "%d", node->cluster );
rtk_fig_text( fig, ox, oy, 0.0, text );
} else {
assert( node->children[0] != NULL );
assert( node->children[1] != NULL );
pf_kdtree_draw_node( self, node->children[0], fig );
pf_kdtree_draw_node( self, node->children[1], fig );
}
}
#endif
1 /*
* Player - One Hell of a Robot Server
* Copyright ( C ) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Useful pdf functions
* Author: Andrew Howard
* Date: 10 Dec 2002
* CVS: $Id: pf_pdf.c 6348 2008-04-17 02:53:17Z gerkey $
*************************************************************************/
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
// #include <gsl/gsl_rng.h>
// #include <gsl/gsl_randist.h>
#include "nav2_amcl/pf/pf_pdf.hpp"
#include "nav2_amcl/portable_utils.hpp"
// Random number generator seed value
static unsigned int pf_pdf_seed;
/**************************************************************************
* Gaussian
*************************************************************************/
// Create a gaussian pdf
48 pf_pdf_gaussian_t * pf_pdf_gaussian_alloc( pf_vector_t x, pf_matrix_t cx )
{
pf_matrix_t cd;
pf_pdf_gaussian_t * pdf;
pdf = calloc( 1, sizeof( pf_pdf_gaussian_t ) );
pdf->x = x;
pdf->cx = cx;
// pdf->cxi = pf_matrix_inverse( cx, &pdf->cxdet );
// Decompose the convariance matrix into a rotation
// matrix and a diagonal matrix.
pf_matrix_unitary( &pdf->cr, &cd, pdf->cx );
pdf->cd.v[0] = sqrt( cd.m[0][0] );
pdf->cd.v[1] = sqrt( cd.m[1][1] );
pdf->cd.v[2] = sqrt( cd.m[2][2] );
// Initialize the random number generator
// pdf->rng = gsl_rng_alloc( gsl_rng_taus );
// gsl_rng_set( pdf->rng, ++pf_pdf_seed );
srand48( ++pf_pdf_seed );
return pdf;
}
// Destroy the pdf
76 void pf_pdf_gaussian_free( pf_pdf_gaussian_t * pdf )
{
// gsl_rng_free( pdf->rng );
free( pdf );
}
/*
// Compute the value of the pdf at some point [x].
double pf_pdf_gaussian_value( pf_pdf_gaussian_t *pdf, pf_vector_t x )
{
int i, j;
pf_vector_t z;
double zz, p;
z = pf_vector_sub( x, pdf->x );
zz = 0;
for ( i = 0; i < 3; i++ )
for ( j = 0; j < 3; j++ )
zz += z.v[i] * pdf->cxi.m[i][j] * z.v[j];
p = 1 / ( 2 * M_PI * pdf->cxdet ) * exp( -zz / 2 );
return p;
}
*/
// Generate a sample from the pdf.
106 pf_vector_t pf_pdf_gaussian_sample( pf_pdf_gaussian_t * pdf )
{
int i, j;
pf_vector_t r;
pf_vector_t x;
// Generate a random vector
for ( i = 0; i < 3; i++ ) {
// r.v[i] = gsl_ran_gaussian( pdf->rng, pdf->cd.v[i] );
r.v[i] = pf_ran_gaussian( pdf->cd.v[i] );
}
for ( i = 0; i < 3; i++ ) {
x.v[i] = pdf->x.v[i];
for ( j = 0; j < 3; j++ ) {
x.v[i] += pdf->cr.m[i][j] * r.v[j];
}
}
return x;
}
// Draw randomly from a zero-mean Gaussian distribution, with standard
// deviation sigma.
// We use the polar form of the Box-Muller transformation, explained here:
// http://www.taygeta.com/random/gaussian.html
132 double pf_ran_gaussian( double sigma )
{
double x1, x2, w, r;
do {
do {
r = drand48( );
} while ( r == 0.0 );
x1 = 2.0 * r - 1.0;
do {
r = drand48( );
} while ( r == 0.0 );
x2 = 2.0 * r - 1.0;
w = x1 * x1 + x2 * x2;
} while ( w > 1.0 || w == 0.0 );
return sigma * x2 * sqrt( -2.0 * log( w ) / w );
}
1 /*
* Player - One Hell of a Robot Server
* Copyright ( C ) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Vector functions
* Author: Andrew Howard
* Date: 10 Dec 2002
* CVS: $Id: pf_vector.c 6345 2008-04-17 01:36:39Z gerkey $
*************************************************************************/
#include <math.h>
// #include <gsl/gsl_matrix.h>
// #include <gsl/gsl_eigen.h>
// #include <gsl/gsl_linalg.h>
#include "nav2_amcl/pf/pf_vector.hpp"
#include "nav2_amcl/pf/eig3.hpp"
// Return a zero vector
38 pf_vector_t pf_vector_zero( )
{
pf_vector_t c;
c.v[0] = 0.0;
c.v[1] = 0.0;
c.v[2] = 0.0;
return c;
}
// // Check for NAN or INF in any component
// int pf_vector_finite( pf_vector_t a )
// {
// int i;
// for ( i = 0; i < 3; i++ ) {
// if ( !isfinite( a.v[i] ) ) {
// return 0;
// }
// }
// return 1;
// }
// Print a vector
// void pf_vector_fprintf( pf_vector_t a, FILE * file, const char * fmt )
// {
// int i;
// for ( i = 0; i < 3; i++ ) {
// fprintf( file, fmt, a.v[i] );
// fprintf( file, " " );
// }
// fprintf( file, "\n" );
// }
// // Simple vector addition
// pf_vector_t pf_vector_add( pf_vector_t a, pf_vector_t b )
// {
// pf_vector_t c;
// c.v[0] = a.v[0] + b.v[0];
// c.v[1] = a.v[1] + b.v[1];
// c.v[2] = a.v[2] + b.v[2];
// return c;
// }
// Simple vector subtraction
92 pf_vector_t pf_vector_sub( pf_vector_t a, pf_vector_t b )
{
pf_vector_t c;
c.v[0] = a.v[0] - b.v[0];
c.v[1] = a.v[1] - b.v[1];
c.v[2] = a.v[2] - b.v[2];
return c;
}
// Transform from local to global coords ( a + b )
105 pf_vector_t pf_vector_coord_add( pf_vector_t a, pf_vector_t b )
{
pf_vector_t c;
c.v[0] = b.v[0] + a.v[0] * cos( b.v[2] ) - a.v[1] * sin( b.v[2] );
c.v[1] = b.v[1] + a.v[0] * sin( b.v[2] ) + a.v[1] * cos( b.v[2] );
c.v[2] = b.v[2] + a.v[2];
c.v[2] = atan2( sin( c.v[2] ), cos( c.v[2] ) );
return c;
}
// // Transform from global to local coords ( a - b )
// pf_vector_t pf_vector_coord_sub( pf_vector_t a, pf_vector_t b )
// {
// pf_vector_t c;
// c.v[0] = +( a.v[0] - b.v[0] ) * cos( b.v[2] ) + ( a.v[1] - b.v[1] ) * sin( b.v[2] );
// c.v[1] = -( a.v[0] - b.v[0] ) * sin( b.v[2] ) + ( a.v[1] - b.v[1] ) * cos( b.v[2] );
// c.v[2] = a.v[2] - b.v[2];
// c.v[2] = atan2( sin( c.v[2] ), cos( c.v[2] ) );
// return c;
// }
// Return a zero matrix
133 pf_matrix_t pf_matrix_zero( )
{
int i, j;
pf_matrix_t c;
for ( i = 0; i < 3; i++ ) {
for ( j = 0; j < 3; j++ ) {
c.m[i][j] = 0.0;
}
}
return c;
}
// // Check for NAN or INF in any component
// int pf_matrix_finite( pf_matrix_t a )
// {
// int i, j;
// for ( i = 0; i < 3; i++ ) {
// for ( j = 0; j < 3; j++ ) {
// if ( !isfinite( a.m[i][j] ) ) {
// return 0;
// }
// }
// }
// return 1;
// }
// Print a matrix
// void pf_matrix_fprintf( pf_matrix_t a, FILE * file, const char * fmt )
// {
// int i, j;
// for ( i = 0; i < 3; i++ ) {
// for ( j = 0; j < 3; j++ ) {
// fprintf( file, fmt, a.m[i][j] );
// fprintf( file, " " );
// }
// fprintf( file, "\n" );
// }
// }
/*
// Compute the matrix inverse
pf_matrix_t pf_matrix_inverse( pf_matrix_t a, double *det )
{
double lndet;
int signum;
gsl_permutation *p;
gsl_matrix_view A, Ai;
pf_matrix_t ai;
A = gsl_matrix_view_array( ( double* ) a.m, 3, 3 );
Ai = gsl_matrix_view_array( ( double* ) ai.m, 3, 3 );
// Do LU decomposition
p = gsl_permutation_alloc( 3 );
gsl_linalg_LU_decomp( &A.matrix, p, &signum );
// Check for underflow
lndet = gsl_linalg_LU_lndet( &A.matrix );
if ( lndet < -1000 )
{
//printf( "underflow in matrix inverse lndet = %f", lndet );
gsl_matrix_set_zero( &Ai.matrix );
}
else
{
// Compute inverse
gsl_linalg_LU_invert( &A.matrix, p, &Ai.matrix );
}
gsl_permutation_free( p );
if ( det )
*det = exp( lndet );
return ai;
}
*/
// Decompose a covariance matrix [a] into a rotation matrix [r] and a diagonal
// matrix [d] such that a = r d r^T.
223 void pf_matrix_unitary( pf_matrix_t * r, pf_matrix_t * d, pf_matrix_t a )
{
int i, j;
/*
gsl_matrix *aa;
gsl_vector *eval;
gsl_matrix *evec;
gsl_eigen_symmv_workspace *w;
aa = gsl_matrix_alloc( 3, 3 );
eval = gsl_vector_alloc( 3 );
evec = gsl_matrix_alloc( 3, 3 );
*/
double aa[3][3];
double eval[3];
double evec[3][3];
for ( i = 0; i < 3; i++ ) {
for ( j = 0; j < 3; j++ ) {
// gsl_matrix_set( aa, i, j, a.m[i][j] );
aa[i][j] = a.m[i][j];
}
}
// Compute eigenvectors/values
/*
w = gsl_eigen_symmv_alloc( 3 );
gsl_eigen_symmv( aa, eval, evec, w );
gsl_eigen_symmv_free( w );
*/
eigen_decomposition( aa, evec, eval );
*d = pf_matrix_zero( );
for ( i = 0; i < 3; i++ ) {
// d->m[i][i] = gsl_vector_get( eval, i );
d->m[i][i] = eval[i];
for ( j = 0; j < 3; j++ ) {
// r->m[i][j] = gsl_matrix_get( evec, i, j );
r->m[i][j] = evec[i][j];
}
}
// gsl_matrix_free( evec );
// gsl_vector_free( eval );
// gsl_matrix_free( aa );
}
1 /*
* Player - One Hell of a Robot Server
* Copyright ( C ) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
#include <math.h>
#include <assert.h>
#include "nav2_amcl/sensors/laser/laser.hpp"
namespace nav2_amcl
{
30 BeamModel::BeamModel(
double z_hit, double z_short, double z_max, double z_rand, double sigma_hit,
32 double lambda_short, double chi_outlier, size_t max_beams, map_t * map )
: Laser( max_beams, map )
{
z_hit_ = z_hit;
z_rand_ = z_rand;
sigma_hit_ = sigma_hit;
z_short_ = z_short;
z_max_ = z_max;
lambda_short_ = lambda_short;
chi_outlier_ = chi_outlier;
}
// Determine the probability for the given pose
double
46 BeamModel::sensorFunction( LaserData * data, pf_sample_set_t * set )
{
BeamModel * self;
int i, j, step;
double z, pz;
double p;
double map_range;
double obs_range, obs_bearing;
double total_weight;
pf_sample_t * sample;
pf_vector_t pose;
self = reinterpret_cast<BeamModel *>( data->laser );
total_weight = 0.0;
// Compute the sample weights
for ( j = 0; j < set->sample_count; j++ ) {
sample = set->samples + j;
pose = sample->pose;
// Take account of the laser pose relative to the robot
pose = pf_vector_coord_add( self->laser_pose_, pose );
p = 1.0;
step = ( data->range_count - 1 ) / ( self->max_beams_ - 1 );
for ( i = 0; i < data->range_count; i += step ) {
obs_range = data->ranges[i][0];
obs_bearing = data->ranges[i][1];
// Compute the range according to the map
map_range = map_calc_range(
self->map_, pose.v[0], pose.v[1],
pose.v[2] + obs_bearing, data->range_max );
pz = 0.0;
// Part 1: good, but noisy, hit
z = obs_range - map_range;
pz += self->z_hit_ * exp( -( z * z ) / ( 2 * self->sigma_hit_ * self->sigma_hit_ ) );
// Part 2: short reading from unexpected obstacle ( e.g., a person )
if ( z < 0 ) {
pz += self->z_short_ * self->lambda_short_ * exp( -self->lambda_short_ * obs_range );
}
// Part 3: Failure to detect obstacle, reported as max-range
if ( obs_range == data->range_max ) {
pz += self->z_max_ * 1.0;
}
// Part 4: Random measurements
if ( obs_range < data->range_max ) {
pz += self->z_rand_ * 1.0 / data->range_max;
}
// TODO( ? ): outlier rejection for short readings
assert( pz <= 1.0 );
assert( pz >= 0.0 );
// p *= pz;
// here we have an ad-hoc weighting scheme for combining beam probs
// works well, though...
p += pz * pz * pz;
}
sample->weight *= p;
total_weight += sample->weight;
}
return total_weight;
}
bool
120 BeamModel::sensorUpdate( pf_t * pf, LaserData * data )
{
if ( max_beams_ < 2 ) {
return false;
}
pf_update_sensor( pf, ( pf_sensor_model_fn_t ) sensorFunction, data );
return true;
}
} // namespace nav2_amcl
1 /*
* Player - One Hell of a Robot Server
* Copyright ( C ) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
#include <sys/types.h>
#include <math.h>
#include <stdlib.h>
#include <assert.h>
#include "nav2_amcl/sensors/laser/laser.hpp"
namespace nav2_amcl
{
32 Laser::Laser( size_t max_beams, map_t * map )
: max_samples_( 0 ), max_obs_( 0 ), temp_obs_( NULL )
{
max_beams_ = max_beams;
map_ = map;
}
39 Laser::~Laser( )
{
if ( temp_obs_ ) {
for ( int k = 0; k < max_samples_; k++ ) {
delete[] temp_obs_[k];
}
delete[] temp_obs_;
}
}
void
50 Laser::reallocTempData( int new_max_samples, int new_max_obs )
{
if ( temp_obs_ ) {
for ( int k = 0; k < max_samples_; k++ ) {
delete[] temp_obs_[k];
}
delete[] temp_obs_;
}
max_obs_ = new_max_obs;
max_samples_ = fmax( max_samples_, new_max_samples );
temp_obs_ = new double *[max_samples_]( );
for ( int k = 0; k < max_samples_; k++ ) {
temp_obs_[k] = new double[max_obs_]( );
}
}
void
68 Laser::SetLaserPose( pf_vector_t & laser_pose )
{
laser_pose_ = laser_pose;
}
} // namespace nav2_amcl
1 /*
* Player - One Hell of a Robot Server
* Copyright ( C ) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
#include <math.h>
#include <assert.h>
#include "nav2_amcl/sensors/laser/laser.hpp"
namespace nav2_amcl
{
30 LikelihoodFieldModel::LikelihoodFieldModel(
double z_hit, double z_rand, double sigma_hit,
32 double max_occ_dist, size_t max_beams, map_t * map )
: Laser( max_beams, map )
{
z_hit_ = z_hit;
z_rand_ = z_rand;
sigma_hit_ = sigma_hit;
map_update_cspace( map, max_occ_dist );
}
double
42 LikelihoodFieldModel::sensorFunction( LaserData * data, pf_sample_set_t * set )
{
LikelihoodFieldModel * self;
int i, j, step;
double z, pz;
double p;
double obs_range, obs_bearing;
double total_weight;
pf_sample_t * sample;
pf_vector_t pose;
pf_vector_t hit;
self = reinterpret_cast<LikelihoodFieldModel *>( data->laser );
total_weight = 0.0;
// Compute the sample weights
for ( j = 0; j < set->sample_count; j++ ) {
sample = set->samples + j;
pose = sample->pose;
// Take account of the laser pose relative to the robot
pose = pf_vector_coord_add( self->laser_pose_, pose );
p = 1.0;
// Pre-compute a couple of things
double z_hit_denom = 2 * self->sigma_hit_ * self->sigma_hit_;
double z_rand_mult = 1.0 / data->range_max;
step = ( data->range_count - 1 ) / ( self->max_beams_ - 1 );
// Step size must be at least 1
if ( step < 1 ) {
step = 1;
}
for ( i = 0; i < data->range_count; i += step ) {
obs_range = data->ranges[i][0];
obs_bearing = data->ranges[i][1];
// This model ignores max range readings
if ( obs_range >= data->range_max ) {
continue;
}
// Check for NaN
if ( obs_range != obs_range ) {
continue;
}
pz = 0.0;
// Compute the endpoint of the beam
hit.v[0] = pose.v[0] + obs_range * cos( pose.v[2] + obs_bearing );
hit.v[1] = pose.v[1] + obs_range * sin( pose.v[2] + obs_bearing );
// Convert to map grid coords.
int mi, mj;
mi = MAP_GXWX( self->map_, hit.v[0] );
mj = MAP_GYWY( self->map_, hit.v[1] );
// Part 1: Get distance from the hit to closest obstacle.
// Off-map penalized as max distance
if ( !MAP_VALID( self->map_, mi, mj ) ) {
z = self->map_->max_occ_dist;
} else {
z = self->map_->cells[MAP_INDEX( self->map_, mi, mj )].occ_dist;
}
// Gaussian model
// NOTE: this should have a normalization of 1/( sqrt( 2pi )*sigma )
pz += self->z_hit_ * exp( -( z * z ) / z_hit_denom );
// Part 2: random measurements
pz += self->z_rand_ * z_rand_mult;
// TODO( ? ): outlier rejection for short readings
assert( pz <= 1.0 );
assert( pz >= 0.0 );
// p *= pz;
// here we have an ad-hoc weighting scheme for combining beam probs
// works well, though...
p += pz * pz * pz;
}
sample->weight *= p;
total_weight += sample->weight;
}
return total_weight;
}
bool
136 LikelihoodFieldModel::sensorUpdate( pf_t * pf, LaserData * data )
{
if ( max_beams_ < 2 ) {
return false;
}
pf_update_sensor( pf, ( pf_sensor_model_fn_t ) sensorFunction, data );
return true;
}
} // namespace nav2_amcl
1 /*
* Player - One Hell of a Robot Server
* Copyright ( C ) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or ( at your option ) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
#include <math.h>
#include <assert.h>
#include "nav2_amcl/sensors/laser/laser.hpp"
namespace nav2_amcl
{
31 LikelihoodFieldModelProb::LikelihoodFieldModelProb(
double z_hit, double z_rand, double sigma_hit,
33 double max_occ_dist, bool do_beamskip,
double beam_skip_distance,
double beam_skip_threshold,
double beam_skip_error_threshold,
37 size_t max_beams, map_t * map )
: Laser( max_beams, map )
{
z_hit_ = z_hit;
z_rand_ = z_rand;
sigma_hit_ = sigma_hit;
do_beamskip_ = do_beamskip;
beam_skip_distance_ = beam_skip_distance;
beam_skip_threshold_ = beam_skip_threshold;
beam_skip_error_threshold_ = beam_skip_error_threshold;
map_update_cspace( map, max_occ_dist );
}
// Determine the probability for the given pose
double
52 LikelihoodFieldModelProb::sensorFunction( LaserData * data, pf_sample_set_t * set )
{
LikelihoodFieldModelProb * self;
int i, j, step;
double z, pz;
double log_p;
double obs_range, obs_bearing;
double total_weight;
pf_sample_t * sample;
pf_vector_t pose;
pf_vector_t hit;
self = reinterpret_cast<LikelihoodFieldModelProb *>( data->laser );
total_weight = 0.0;
step = ceil( ( data->range_count ) / static_cast<double>( self->max_beams_ ) );
// Step size must be at least 1
if ( step < 1 ) {
step = 1;
}
// Pre-compute a couple of things
double z_hit_denom = 2 * self->sigma_hit_ * self->sigma_hit_;
double z_rand_mult = 1.0 / data->range_max;
double max_dist_prob = exp( -( self->map_->max_occ_dist * self->map_->max_occ_dist ) / z_hit_denom );
// Beam skipping - ignores beams for which a majoirty of particles do not agree with the map
// prevents correct particles from getting down weighted because of unexpected obstacles
// such as humans
bool do_beamskip = self->do_beamskip_;
double beam_skip_distance = self->beam_skip_distance_;
double beam_skip_threshold = self->beam_skip_threshold_;
// we only do beam skipping if the filter has converged
if ( do_beamskip && !set->converged ) {
do_beamskip = false;
}
// we need a count the no of particles for which the beam agreed with the map
int * obs_count = new int[self->max_beams_]( );
// we also need a mask of which observations to integrate ( to decide which beams to integrate to
// all particles )
bool * obs_mask = new bool[self->max_beams_]( );
int beam_ind = 0;
// realloc indicates if we need to reallocate the temp data structure needed to do beamskipping
bool realloc = false;
if ( do_beamskip ) {
if ( self->max_obs_ < self->max_beams_ ) {
realloc = true;
}
if ( self->max_samples_ < set->sample_count ) {
realloc = true;
}
if ( realloc ) {
self->reallocTempData( set->sample_count, self->max_beams_ );
fprintf( stderr, "Reallocing temp weights %d - %d\n", self->max_samples_, self->max_obs_ );
}
}
// Compute the sample weights
for ( j = 0; j < set->sample_count; j++ ) {
sample = set->samples + j;
pose = sample->pose;
// Take account of the laser pose relative to the robot
pose = pf_vector_coord_add( self->laser_pose_, pose );
log_p = 0;
beam_ind = 0;
for ( i = 0; i < data->range_count; i += step, beam_ind++ ) {
obs_range = data->ranges[i][0];
obs_bearing = data->ranges[i][1];
// This model ignores max range readings
if ( obs_range >= data->range_max ) {
continue;
}
// Check for NaN
if ( obs_range != obs_range ) {
continue;
}
pz = 0.0;
// Compute the endpoint of the beam
hit.v[0] = pose.v[0] + obs_range * cos( pose.v[2] + obs_bearing );
hit.v[1] = pose.v[1] + obs_range * sin( pose.v[2] + obs_bearing );
// Convert to map grid coords.
int mi, mj;
mi = MAP_GXWX( self->map_, hit.v[0] );
mj = MAP_GYWY( self->map_, hit.v[1] );
// Part 1: Get distance from the hit to closest obstacle.
// Off-map penalized as max distance
if ( !MAP_VALID( self->map_, mi, mj ) ) {
pz += self->z_hit_ * max_dist_prob;
} else {
z = self->map_->cells[MAP_INDEX( self->map_, mi, mj )].occ_dist;
if ( z < beam_skip_distance ) {
obs_count[beam_ind] += 1;
}
pz += self->z_hit_ * exp( -( z * z ) / z_hit_denom );
}
// Gaussian model
// NOTE: this should have a normalization of 1/( sqrt( 2pi )*sigma )
// Part 2: random measurements
pz += self->z_rand_ * z_rand_mult;
assert( pz <= 1.0 );
assert( pz >= 0.0 );
// TODO( ? ): outlier rejection for short readings
if ( !do_beamskip ) {
log_p += log( pz );
} else {
self->temp_obs_[j][beam_ind] = pz;
}
}
if ( !do_beamskip ) {
sample->weight *= exp( log_p );
total_weight += sample->weight;
}
}
if ( do_beamskip ) {
int skipped_beam_count = 0;
for ( beam_ind = 0; beam_ind < self->max_beams_; beam_ind++ ) {
if ( ( obs_count[beam_ind] / static_cast<double>( set->sample_count ) ) > beam_skip_threshold ) {
obs_mask[beam_ind] = true;
} else {
obs_mask[beam_ind] = false;
skipped_beam_count++;
}
}
// we check if there is at least a critical number of beams that agreed with the map
// otherwise it probably indicates that the filter converged to a wrong solution
// if that's the case we integrate all the beams and hope the filter might converge to
// the right solution
bool error = false;
if ( skipped_beam_count >= ( beam_ind * self->beam_skip_error_threshold_ ) ) {
fprintf(
stderr,
"Over %f%% of the observations were not in the map - pf may have converged to wrong pose -"
" integrating all observations\n",
( 100 * self->beam_skip_error_threshold_ ) );
error = true;
}
for ( j = 0; j < set->sample_count; j++ ) {
sample = set->samples + j;
pose = sample->pose;
log_p = 0;
for ( beam_ind = 0; beam_ind < self->max_beams_; beam_ind++ ) {
if ( error || obs_mask[beam_ind] ) {
log_p += log( self->temp_obs_[j][beam_ind] );
}
}
sample->weight *= exp( log_p );
total_weight += sample->weight;
}
}
delete[] obs_count;
delete[] obs_mask;
return total_weight;
}
bool
244 LikelihoodFieldModelProb::sensorUpdate( pf_t * pf, LaserData * data )
{
if ( max_beams_ < 2 ) {
return false;
}
pf_update_sensor( pf, ( pf_sensor_model_fn_t ) sensorFunction, data );
return true;
}
} // namespace nav2_amcl
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Florian Gramss
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__BEHAVIOR_TREE_ENGINE_HPP_
#define NAV2_BEHAVIOR_TREE__BEHAVIOR_TREE_ENGINE_HPP_
#include <memory>
#include <string>
#include <vector>
#include "behaviortree_cpp_v3/behavior_tree.h"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behaviortree_cpp_v3/xml_parsing.h"
#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
namespace nav2_behavior_tree
{
/**
* @enum nav2_behavior_tree::BtStatus
* @brief An enum class representing BT execution status
*/
36 enum class BtStatus { SUCCEEDED, FAILED, CANCELED };
/**
* @class nav2_behavior_tree::BehaviorTreeEngine
* @brief A class to create and handle behavior trees
*/
42 class BehaviorTreeEngine
{
public:
/**
* @brief A constructor for nav2_behavior_tree::BehaviorTreeEngine
* @param plugin_libraries vector of BT plugin library names to load
*/
49 explicit BehaviorTreeEngine( const std::vector<std::string> & plugin_libraries );
50 virtual ~BehaviorTreeEngine( ) {}
/**
* @brief Function to execute a BT at a specific rate
* @param tree BT to execute
* @param onLoop Function to execute on each iteration of BT execution
* @param cancelRequested Function to check if cancel was requested during BT execution
* @param loopTimeout Time period for each iteration of BT execution
* @return nav2_behavior_tree::BtStatus Status of BT execution
*/
60 BtStatus run(
BT::Tree * tree,
std::function<void( )> onLoop,
std::function<bool( )> cancelRequested,
std::chrono::milliseconds loopTimeout = std::chrono::milliseconds( 10 ) );
/**
* @brief Function to create a BT from a XML string
* @param xml_string XML string representing BT
* @param blackboard Blackboard for BT
* @return BT::Tree Created behavior tree
*/
72 BT::Tree createTreeFromText(
73 const std::string & xml_string,
74 BT::Blackboard::Ptr blackboard );
/**
* @brief Function to create a BT from an XML file
* @param file_path Path to BT XML file
* @param blackboard Blackboard for BT
* @return BT::Tree Created behavior tree
*/
82 BT::Tree createTreeFromFile(
83 const std::string & file_path,
84 BT::Blackboard::Ptr blackboard );
/**
* @brief Function to explicitly reset all BT nodes to initial state
* @param root_node Pointer to BT root node
*/
90 void haltAllActions( BT::TreeNode * root_node );
protected:
// The factory that will be used to dynamically construct the behavior tree
94 BT::BehaviorTreeFactory factory_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__BEHAVIOR_TREE_ENGINE_HPP_
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_
#define NAV2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_
#include <memory>
#include <string>
#include "behaviortree_cpp_v3/action_node.h"
#include "nav2_util/node_utils.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_behavior_tree/bt_conversions.hpp"
namespace nav2_behavior_tree
{
/**
* @brief Abstract class representing an action based BT node
* @tparam ActionT Type of action
*/
template<class ActionT>
34 class BtActionNode : public BT::ActionNodeBase
{
public:
/**
* @brief A nav2_behavior_tree::BtActionNode constructor
* @param xml_tag_name Name for the XML tag for this node
* @param action_name Action name this node creates a client for
* @param conf BT node configuration
*/
43 BtActionNode(
44 const std::string & xml_tag_name,
45 const std::string & action_name,
46 const BT::NodeConfiguration & conf )
: BT::ActionNodeBase( xml_tag_name, conf ), action_name_( action_name )
{
node_ = config( ).blackboard->template get<rclcpp::Node::SharedPtr>( "node" );
callback_group_ = node_->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive,
false );
callback_group_executor_.add_callback_group( callback_group_, node_->get_node_base_interface( ) );
// Get the required items from the blackboard
bt_loop_duration_ =
config( ).blackboard->template get<std::chrono::milliseconds>( "bt_loop_duration" );
server_timeout_ =
config( ).blackboard->template get<std::chrono::milliseconds>( "server_timeout" );
getInput<std::chrono::milliseconds>( "server_timeout", server_timeout_ );
// Initialize the input and output messages
goal_ = typename ActionT::Goal( );
result_ = typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult( );
std::string remapped_action_name;
if ( getInput( "server_name", remapped_action_name ) ) {
action_name_ = remapped_action_name;
}
createActionClient( action_name_ );
// Give the derive class a chance to do any initialization
RCLCPP_DEBUG( node_->get_logger( ), "\"%s\" BtActionNode initialized", xml_tag_name.c_str( ) );
}
BtActionNode( ) = delete;
virtual ~BtActionNode( )
{
}
/**
* @brief Create instance of an action client
* @param action_name Action name to create client for
*/
void createActionClient( const std::string & action_name )
{
// Now that we have the ROS node to use, create the action client for this BT action
action_client_ = rclcpp_action::create_client<ActionT>( node_, action_name, callback_group_ );
// Make sure the server is actually there before continuing
92 RCLCPP_DEBUG( node_->get_logger( ), "Waiting for \"%s\" action server", action_name.c_str( ) );
static constexpr std::chrono::milliseconds wait_for_server_timeout =
std::chrono::milliseconds( 1000 );
95 if ( !action_client_->wait_for_action_server( wait_for_server_timeout ) ) {
RCLCPP_ERROR(
node_->get_logger( ), "\"%s\" action server not available after waiting for %li ms",
action_name.c_str( ), wait_for_server_timeout.count( ) );
throw std::runtime_error( "Action server not available" );
}
}
/**
* @brief Any subclass of BtActionNode that accepts parameters must provide a
* providedPorts method and call providedBasicPorts in it.
* @param addition Additional ports to add to BT port list
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
static BT::PortsList providedBasicPorts( BT::PortsList addition )
{
BT::PortsList basic = {
BT::InputPort<std::string>( "server_name", "Action server name" ),
BT::InputPort<std::chrono::milliseconds>( "server_timeout" )
};
basic.insert( addition.begin( ), addition.end( ) );
return basic;
}
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
static BT::PortsList providedPorts( )
{
return providedBasicPorts( {} );
}
// Derived classes can override any of the following methods to hook into the
// processing for the action: on_tick, on_wait_for_result, and on_success
/**
* @brief Function to perform some user-defined operation on tick
* Could do dynamic checks, such as getting updates to values on the blackboard
*/
virtual void on_tick( )
{
}
/**
* @brief Function to perform some user-defined operation after a timeout
* waiting for a result that hasn't been received yet. Also provides access to
* the latest feedback message from the action server. Feedback will be nullptr
* in subsequent calls to this function if no new feedback is received while waiting for a result.
* @param feedback shared_ptr to latest feedback message, nullptr if no new feedback was received
*/
virtual void on_wait_for_result( std::shared_ptr<const typename ActionT::Feedback>/*feedback*/ )
{
}
/**
* @brief Function to perform some user-defined operation upon successful
* completion of the action. Could put a value on the blackboard.
* @return BT::NodeStatus Returns SUCCESS by default, user may override return another value
*/
virtual BT::NodeStatus on_success( )
{
return BT::NodeStatus::SUCCESS;
}
/**
* @brief Function to perform some user-defined operation whe the action is aborted.
* @return BT::NodeStatus Returns FAILURE by default, user may override return another value
*/
virtual BT::NodeStatus on_aborted( )
{
return BT::NodeStatus::FAILURE;
}
/**
* @brief Function to perform some user-defined operation when the action is cancelled.
* @return BT::NodeStatus Returns SUCCESS by default, user may override return another value
*/
virtual BT::NodeStatus on_cancelled( )
{
return BT::NodeStatus::SUCCESS;
}
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
BT::NodeStatus tick( ) override
{
// first step to be done only at the beginning of the Action
if ( status( ) == BT::NodeStatus::IDLE ) {
// setting the status to RUNNING to notify the BT Loggers ( if any )
setStatus( BT::NodeStatus::RUNNING );
// user defined callback
on_tick( );
send_new_goal( );
}
try {
// if new goal was sent and action server has not yet responded
// check the future goal handle
if ( future_goal_handle_ ) {
auto elapsed = ( node_->now( ) - time_goal_sent_ ).to_chrono<std::chrono::milliseconds>( );
if ( !is_future_goal_handle_complete( elapsed ) ) {
// return RUNNING if there is still some time before timeout happens
if ( elapsed < server_timeout_ ) {
return BT::NodeStatus::RUNNING;
}
// if server has taken more time than the specified timeout value return FAILURE
RCLCPP_WARN(
node_->get_logger( ),
"Timed out while waiting for action server to acknowledge goal request for %s",
action_name_.c_str( ) );
future_goal_handle_.reset( );
return BT::NodeStatus::FAILURE;
}
}
// The following code corresponds to the "RUNNING" loop
if ( rclcpp::ok( ) && !goal_result_available_ ) {
// user defined callback. May modify the value of "goal_updated_"
on_wait_for_result( feedback_ );
// reset feedback to avoid stale information
feedback_.reset( );
auto goal_status = goal_handle_->get_status( );
if ( goal_updated_ && ( goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING ||
goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED ) )
{
goal_updated_ = false;
send_new_goal( );
auto elapsed = ( node_->now( ) - time_goal_sent_ ).to_chrono<std::chrono::milliseconds>( );
if ( !is_future_goal_handle_complete( elapsed ) ) {
if ( elapsed < server_timeout_ ) {
return BT::NodeStatus::RUNNING;
}
RCLCPP_WARN(
node_->get_logger( ),
"Timed out while waiting for action server to acknowledge goal request for %s",
action_name_.c_str( ) );
future_goal_handle_.reset( );
return BT::NodeStatus::FAILURE;
}
}
callback_group_executor_.spin_some( );
// check if, after invoking spin_some( ), we finally received the result
if ( !goal_result_available_ ) {
// Yield this Action, returning RUNNING
return BT::NodeStatus::RUNNING;
}
}
} catch ( const std::runtime_error & e ) {
if ( e.what( ) == std::string( "send_goal failed" ) ||
e.what( ) == std::string( "Goal was rejected by the action server" ) )
{
// Action related failure that should not fail the tree, but the node
return BT::NodeStatus::FAILURE;
} else {
// Internal exception to propogate to the tree
throw e;
}
}
BT::NodeStatus status;
switch ( result_.code ) {
case rclcpp_action::ResultCode::SUCCEEDED:
status = on_success( );
break;
case rclcpp_action::ResultCode::ABORTED:
status = on_aborted( );
break;
case rclcpp_action::ResultCode::CANCELED:
status = on_cancelled( );
break;
default:
throw std::logic_error( "BtActionNode::Tick: invalid status value" );
}
goal_handle_.reset( );
return status;
}
/**
* @brief The other ( optional ) override required by a BT action. In this case, we
* make sure to cancel the ROS2 action if it is still running.
*/
void halt( ) override
{
if ( should_cancel_goal( ) ) {
auto future_cancel = action_client_->async_cancel_goal( goal_handle_ );
if ( callback_group_executor_.spin_until_future_complete( future_cancel, server_timeout_ ) !=
rclcpp::FutureReturnCode::SUCCESS )
{
RCLCPP_ERROR(
node_->get_logger( ),
"Failed to cancel action server for %s", action_name_.c_str( ) );
}
}
setStatus( BT::NodeStatus::IDLE );
}
protected:
/**
* @brief Function to check if current goal should be cancelled
* @return bool True if current goal should be cancelled, false otherwise
*/
bool should_cancel_goal( )
{
// Shut the node down if it is currently running
if ( status( ) != BT::NodeStatus::RUNNING ) {
return false;
}
// No need to cancel the goal if goal handle is invalid
if ( !goal_handle_ ) {
return false;
}
callback_group_executor_.spin_some( );
auto status = goal_handle_->get_status( );
// Check if the goal is still executing
return status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED ||
status == action_msgs::msg::GoalStatus::STATUS_EXECUTING;
}
/**
* @brief Function to send new goal to action server
*/
void send_new_goal( )
{
goal_result_available_ = false;
auto send_goal_options = typename rclcpp_action::Client<ActionT>::SendGoalOptions( );
send_goal_options.result_callback =
[this]( const typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult & result ) {
if ( future_goal_handle_ ) {
RCLCPP_DEBUG(
node_->get_logger( ),
"Goal result for %s available, but it hasn't received the goal response yet. "
"It's probably a goal result for the last goal request", action_name_.c_str( ) );
return;
}
// TODO( #1652 ): a work around until rcl_action interface is updated
// if goal ids are not matched, the older goal call this callback so ignore the result
// if matched, it must be processed ( including aborted )
if ( this->goal_handle_->get_goal_id( ) == result.goal_id ) {
goal_result_available_ = true;
result_ = result;
}
};
send_goal_options.feedback_callback =
[this]( typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr,
const std::shared_ptr<const typename ActionT::Feedback> feedback ) {
feedback_ = feedback;
};
future_goal_handle_ = std::make_shared<
std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>(
action_client_->async_send_goal( goal_, send_goal_options ) );
time_goal_sent_ = node_->now( );
}
/**
* @brief Function to check if the action server acknowledged a new goal
* @param elapsed Duration since the last goal was sent and future goal handle has not completed.
* After waiting for the future to complete, this value is incremented with the timeout value.
* @return boolean True if future_goal_handle_ returns SUCCESS, False otherwise
*/
bool is_future_goal_handle_complete( std::chrono::milliseconds & elapsed )
{
auto remaining = server_timeout_ - elapsed;
// server has already timed out, no need to sleep
if ( remaining <= std::chrono::milliseconds( 0 ) ) {
future_goal_handle_.reset( );
return false;
}
auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining;
auto result =
callback_group_executor_.spin_until_future_complete( *future_goal_handle_, timeout );
elapsed += timeout;
if ( result == rclcpp::FutureReturnCode::INTERRUPTED ) {
future_goal_handle_.reset( );
throw std::runtime_error( "send_goal failed" );
}
if ( result == rclcpp::FutureReturnCode::SUCCESS ) {
goal_handle_ = future_goal_handle_->get( );
future_goal_handle_.reset( );
if ( !goal_handle_ ) {
throw std::runtime_error( "Goal was rejected by the action server" );
}
return true;
}
return false;
}
/**
* @brief Function to increment recovery count on blackboard if this node wraps a recovery
*/
void increment_recovery_count( )
{
int recovery_count = 0;
config( ).blackboard->template get<int>( "number_recoveries", recovery_count ); // NOLINT
recovery_count += 1;
config( ).blackboard->template set<int>( "number_recoveries", recovery_count ); // NOLINT
}
std::string action_name_;
typename std::shared_ptr<rclcpp_action::Client<ActionT>> action_client_;
// All ROS2 actions have a goal and a result
typename ActionT::Goal goal_;
bool goal_updated_{false};
bool goal_result_available_{false};
typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr goal_handle_;
typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult result_;
// To handle feedback from action server
std::shared_ptr<const typename ActionT::Feedback> feedback_;
// The node that will be used for any ROS operations
rclcpp::Node::SharedPtr node_;
rclcpp::CallbackGroup::SharedPtr callback_group_;
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
// The timeout value while waiting for response from a server when a
// new action goal is sent or canceled
std::chrono::milliseconds server_timeout_;
// The timeout value for BT loop execution
std::chrono::milliseconds bt_loop_duration_;
// To track the action server acknowledgement when a new goal is sent
std::shared_ptr<std::shared_future<typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr>>
future_goal_handle_;
rclcpp::Time time_goal_sent_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__BT_ACTION_NODE_HPP_
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_HPP_
#define NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_HPP_
#include <memory>
#include <string>
#include <vector>
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_behavior_tree/behavior_tree_engine.hpp"
#include "nav2_behavior_tree/ros_topic_logger.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/simple_action_server.hpp"
namespace nav2_behavior_tree
{
/**
* @class nav2_behavior_tree::BtActionServer
* @brief An action server that uses behavior tree to execute an action
*/
template<class ActionT>
35 class BtActionServer
{
public:
using ActionServer = nav2_util::SimpleActionServer<ActionT>;
typedef std::function<bool ( typename ActionT::Goal::ConstSharedPtr )> OnGoalReceivedCallback;
typedef std::function<void ( )> OnLoopCallback;
typedef std::function<void ( typename ActionT::Goal::ConstSharedPtr )> OnPreemptCallback;
typedef std::function<void ( typename ActionT::Result::SharedPtr,
nav2_behavior_tree::BtStatus )> OnCompletionCallback;
/**
* @brief A constructor for nav2_behavior_tree::BtActionServer class
*/
explicit BtActionServer(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
const std::string & action_name,
const std::vector<std::string> & plugin_lib_names,
const std::string & default_bt_xml_filename,
OnGoalReceivedCallback on_goal_received_callback,
OnLoopCallback on_loop_callback,
OnPreemptCallback on_preempt_callback,
OnCompletionCallback on_completion_callback );
/**
* @brief A destructor for nav2_behavior_tree::BtActionServer class
*/
~BtActionServer( );
/**
* @brief Configures member variables
* Initializes action server for, builds behavior tree from xml file,
* and calls user-defined onConfigure.
* @return bool true on SUCCESS and false on FAILURE
*/
bool on_configure( );
/**
* @brief Activates action server
* @return bool true on SUCCESS and false on FAILURE
*/
bool on_activate( );
/**
* @brief Deactivates action server
* @return bool true on SUCCESS and false on FAILURE
*/
bool on_deactivate( );
/**
* @brief Resets member variables
* @return bool true on SUCCESS and false on FAILURE
*/
bool on_cleanup( );
/**
* @brief Replace current BT with another one
* @param bt_xml_filename The file containing the new BT, uses default filename if empty
* @return bool true if the resulting BT correspond to the one in bt_xml_filename. false
* if something went wrong, and previous BT is maintained
*/
bool loadBehaviorTree( const std::string & bt_xml_filename = "" );
/**
* @brief Getter function for BT Blackboard
* @return BT::Blackboard::Ptr Shared pointer to current BT blackboard
*/
BT::Blackboard::Ptr getBlackboard( ) const
{
return blackboard_;
}
/**
* @brief Getter function for current BT XML filename
* @return string Containing current BT XML filename
*/
111 std::string getCurrentBTFilename( ) const
{
return current_bt_xml_filename_;
}
/**
* @brief Getter function for default BT XML filename
* @return string Containing default BT XML filename
*/
120 std::string getDefaultBTFilename( ) const
{
return default_bt_xml_filename_;
}
/**
* @brief Wrapper function to accept pending goal if a preempt has been requested
* @return Shared pointer to pending action goal
*/
129 const std::shared_ptr<const typename ActionT::Goal> acceptPendingGoal( )
{
return action_server_->accept_pending_goal( );
}
/**
* @brief Wrapper function to terminate pending goal if a preempt has been requested
*/
137 void terminatePendingGoal( )
{
action_server_->terminate_pending_goal( );
}
/**
* @brief Wrapper function to get current goal
* @return Shared pointer to current action goal
*/
146 const std::shared_ptr<const typename ActionT::Goal> getCurrentGoal( ) const
{
return action_server_->get_current_goal( );
}
/**
* @brief Wrapper function to get pending goal
* @return Shared pointer to pending action goal
*/
155 const std::shared_ptr<const typename ActionT::Goal> getPendingGoal( ) const
{
return action_server_->get_pending_goal( );
}
/**
* @brief Wrapper function to publish action feedback
*/
163 void publishFeedback( typename std::shared_ptr<typename ActionT::Feedback> feedback )
{
action_server_->publish_feedback( feedback );
}
/**
* @brief Getter function for the current BT tree
* @return BT::Tree Current behavior tree
*/
172 const BT::Tree & getTree( ) const
{
return tree_;
}
/**
* @brief Function to halt the current tree. It will interrupt the execution of RUNNING nodes
* by calling their halt( ) implementation ( only for Async nodes that may return RUNNING )
*/
181 void haltTree( )
{
tree_.rootNode( )->halt( );
}
protected:
/**
* @brief Action server callback
*/
190 void executeCallback( );
// Action name
std::string action_name_;
// Our action server implements the template action
std::shared_ptr<ActionServer> action_server_;
// Behavior Tree to be executed when goal is received
BT::Tree tree_;
// The blackboard shared by all of the nodes in the tree
BT::Blackboard::Ptr blackboard_;
// The XML file that cointains the Behavior Tree to create
std::string current_bt_xml_filename_;
std::string default_bt_xml_filename_;
// The wrapper class for the BT functionality
std::unique_ptr<nav2_behavior_tree::BehaviorTreeEngine> bt_;
// Libraries to pull plugins ( BT Nodes ) from
std::vector<std::string> plugin_lib_names_;
// A regular, non-spinning ROS node that we can use for calls to the action client
rclcpp::Node::SharedPtr client_node_;
// Parent node
rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
// Clock
rclcpp::Clock::SharedPtr clock_;
// Logger
224 rclcpp::Logger logger_{rclcpp::get_logger( "BtActionServer" )};
// To publish BT logs
std::unique_ptr<RosTopicLogger> topic_logger_;
// Duration for each iteration of BT execution
std::chrono::milliseconds bt_loop_duration_;
// Default timeout value while waiting for response from a server
std::chrono::milliseconds default_server_timeout_;
// User-provided callbacks
OnGoalReceivedCallback on_goal_received_callback_;
OnLoopCallback on_loop_callback_;
OnPreemptCallback on_preempt_callback_;
OnCompletionCallback on_completion_callback_;
};
} // namespace nav2_behavior_tree
#include <nav2_behavior_tree/bt_action_server_impl.hpp> // NOLINT( build/include_order )
#endif // NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_HPP_
1 // Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_IMPL_HPP_
#define NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_IMPL_HPP_
#include <memory>
#include <string>
#include <fstream>
#include <set>
#include <exception>
#include <vector>
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "nav2_behavior_tree/bt_action_server.hpp"
#include "ament_index_cpp/get_package_share_directory.hpp"
namespace nav2_behavior_tree
{
template<class ActionT>
33 BtActionServer<ActionT>::BtActionServer(
34 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
35 const std::string & action_name,
36 const std::vector<std::string> & plugin_lib_names,
37 const std::string & default_bt_xml_filename,
38 OnGoalReceivedCallback on_goal_received_callback,
39 OnLoopCallback on_loop_callback,
40 OnPreemptCallback on_preempt_callback,
41 OnCompletionCallback on_completion_callback )
: action_name_( action_name ),
default_bt_xml_filename_( default_bt_xml_filename ),
plugin_lib_names_( plugin_lib_names ),
node_( parent ),
on_goal_received_callback_( on_goal_received_callback ),
on_loop_callback_( on_loop_callback ),
on_preempt_callback_( on_preempt_callback ),
on_completion_callback_( on_completion_callback )
{
auto node = node_.lock( );
logger_ = node->get_logger( );
clock_ = node->get_clock( );
// Declare this node's parameters
if ( !node->has_parameter( "bt_loop_duration" ) ) {
node->declare_parameter( "bt_loop_duration", 10 );
}
if ( !node->has_parameter( "default_server_timeout" ) ) {
node->declare_parameter( "default_server_timeout", 20 );
}
}
template<class ActionT>
65 BtActionServer<ActionT>::~BtActionServer( )
{}
template<class ActionT>
69 bool BtActionServer<ActionT>::on_configure( )
{
auto node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
// Name client node after action name
std::string client_node_name = action_name_;
std::replace( client_node_name.begin( ), client_node_name.end( ), '/', '_' );
// Use suffix '_rclcpp_node' to keep parameter file consistency #1773
auto options = rclcpp::NodeOptions( ).arguments(
{"--ros-args",
"-r",
std::string( "__node:=" ) +
std::string( node->get_name( ) ) + "_" + client_node_name + "_rclcpp_node",
"--"} );
// Support for handling the topic-based goal pose from rviz
client_node_ = std::make_shared<rclcpp::Node>( "_", options );
action_server_ = std::make_shared<ActionServer>(
node->get_node_base_interface( ),
node->get_node_clock_interface( ),
node->get_node_logging_interface( ),
node->get_node_waitables_interface( ),
action_name_, std::bind( &BtActionServer<ActionT>::executeCallback, this ) );
// Get parameters for BT timeouts
int timeout;
node->get_parameter( "bt_loop_duration", timeout );
bt_loop_duration_ = std::chrono::milliseconds( timeout );
node->get_parameter( "default_server_timeout", timeout );
default_server_timeout_ = std::chrono::milliseconds( timeout );
// Create the class that registers our custom nodes and executes the BT
bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>( plugin_lib_names_ );
// Create the blackboard that will be shared by all of the nodes in the tree
blackboard_ = BT::Blackboard::create( );
// Put items on the blackboard
blackboard_->set<rclcpp::Node::SharedPtr>( "node", client_node_ ); // NOLINT
blackboard_->set<std::chrono::milliseconds>( "server_timeout", default_server_timeout_ ); // NOLINT
blackboard_->set<std::chrono::milliseconds>( "bt_loop_duration", bt_loop_duration_ ); // NOLINT
return true;
}
template<class ActionT>
119 bool BtActionServer<ActionT>::on_activate( )
{
if ( !loadBehaviorTree( default_bt_xml_filename_ ) ) {
RCLCPP_ERROR( logger_, "Error loading XML file: %s", default_bt_xml_filename_.c_str( ) );
return false;
}
action_server_->activate( );
return true;
}
template<class ActionT>
130 bool BtActionServer<ActionT>::on_deactivate( )
{
action_server_->deactivate( );
return true;
}
template<class ActionT>
137 bool BtActionServer<ActionT>::on_cleanup( )
{
client_node_.reset( );
action_server_.reset( );
topic_logger_.reset( );
plugin_lib_names_.clear( );
current_bt_xml_filename_.clear( );
blackboard_.reset( );
bt_->haltAllActions( tree_.rootNode( ) );
bt_.reset( );
return true;
}
template<class ActionT>
151 bool BtActionServer<ActionT>::loadBehaviorTree( const std::string & bt_xml_filename )
{
// Empty filename is default for backward compatibility
auto filename = bt_xml_filename.empty( ) ? default_bt_xml_filename_ : bt_xml_filename;
// Use previous BT if it is the existing one
if ( current_bt_xml_filename_ == filename ) {
RCLCPP_DEBUG( logger_, "BT will not be reloaded as the given xml is already loaded" );
return true;
}
// Read the input BT XML from the specified file into a string
std::ifstream xml_file( filename );
if ( !xml_file.good( ) ) {
RCLCPP_ERROR( logger_, "Couldn't open input XML file: %s", filename.c_str( ) );
return false;
}
auto xml_string = std::string(
std::istreambuf_iterator<char>( xml_file ),
std::istreambuf_iterator<char>( ) );
// Create the Behavior Tree from the XML input
try {
tree_ = bt_->createTreeFromText( xml_string, blackboard_ );
} catch ( const std::exception & e ) {
RCLCPP_ERROR( logger_, "Exception when loading BT: %s", e.what( ) );
return false;
}
topic_logger_ = std::make_unique<RosTopicLogger>( client_node_, tree_ );
current_bt_xml_filename_ = filename;
return true;
}
template<class ActionT>
189 void BtActionServer<ActionT>::executeCallback( )
{
if ( !on_goal_received_callback_( action_server_->get_current_goal( ) ) ) {
action_server_->terminate_current( );
return;
}
auto is_canceling = [&]( ) {
if ( action_server_ == nullptr ) {
RCLCPP_DEBUG( logger_, "Action server unavailable. Canceling." );
return true;
}
if ( !action_server_->is_server_active( ) ) {
RCLCPP_DEBUG( logger_, "Action server is inactive. Canceling." );
return true;
}
return action_server_->is_cancel_requested( );
};
auto on_loop = [&]( ) {
if ( action_server_->is_preempt_requested( ) && on_preempt_callback_ ) {
on_preempt_callback_( action_server_->get_pending_goal( ) );
}
topic_logger_->flush( );
on_loop_callback_( );
};
// Execute the BT that was previously created in the configure step
nav2_behavior_tree::BtStatus rc = bt_->run( &tree_, on_loop, is_canceling, bt_loop_duration_ );
// Make sure that the Bt is not in a running state from a previous execution
// note: if all the ControlNodes are implemented correctly, this is not needed.
bt_->haltAllActions( tree_.rootNode( ) );
// Give server an opportunity to populate the result message or simple give
// an indication that the action is complete.
auto result = std::make_shared<typename ActionT::Result>( );
on_completion_callback_( result, rc );
switch ( rc ) {
case nav2_behavior_tree::BtStatus::SUCCEEDED:
RCLCPP_INFO( logger_, "Goal succeeded" );
action_server_->succeeded_current( result );
break;
case nav2_behavior_tree::BtStatus::FAILED:
RCLCPP_ERROR( logger_, "Goal failed" );
action_server_->terminate_current( result );
break;
case nav2_behavior_tree::BtStatus::CANCELED:
RCLCPP_INFO( logger_, "Goal canceled" );
action_server_->terminate_all( result );
break;
}
}
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_IMPL_HPP_
// Copyright ( c ) 2022 Neobotix GmbH
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__BT_CANCEL_ACTION_NODE_HPP_
#define NAV2_BEHAVIOR_TREE__BT_CANCEL_ACTION_NODE_HPP_
#include <memory>
#include <string>
#include <chrono>
#include "behaviortree_cpp_v3/action_node.h"
#include "nav2_util/node_utils.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_behavior_tree/bt_conversions.hpp"
namespace nav2_behavior_tree
{
/**
* @brief Abstract class representing an action for cancelling BT node
* @tparam ActionT Type of action
*/
template<class ActionT>
35 class BtCancelActionNode : public BT::ActionNodeBase
{
public:
/**
* @brief A nav2_behavior_tree::BtCancelActionNode constructor
* @param xml_tag_name Name for the XML tag for this node
* @param action_name Action name this node creates a client for
* @param conf BT node configuration
*/
44 BtCancelActionNode(
45 const std::string & xml_tag_name,
46 const std::string & action_name,
47 const BT::NodeConfiguration & conf )
: BT::ActionNodeBase( xml_tag_name, conf ), action_name_( action_name )
{
node_ = config( ).blackboard->template get<rclcpp::Node::SharedPtr>( "node" );
callback_group_ = node_->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive,
false );
callback_group_executor_.add_callback_group( callback_group_, node_->get_node_base_interface( ) );
// Get the required items from the blackboard
server_timeout_ =
config( ).blackboard->template get<std::chrono::milliseconds>( "server_timeout" );
getInput<std::chrono::milliseconds>( "server_timeout", server_timeout_ );
std::string remapped_action_name;
if ( getInput( "server_name", remapped_action_name ) ) {
action_name_ = remapped_action_name;
}
createActionClient( action_name_ );
// Give the derive class a chance to do any initialization
RCLCPP_DEBUG(
node_->get_logger( ), "\"%s\" BtCancelActionNode initialized",
xml_tag_name.c_str( ) );
}
BtCancelActionNode( ) = delete;
virtual ~BtCancelActionNode( )
{
}
/**
* @brief Create instance of an action client
* @param action_name Action name to create client for
*/
void createActionClient( const std::string & action_name )
{
// Now that we have the ROS node to use, create the action client for this BT action
action_client_ = rclcpp_action::create_client<ActionT>( node_, action_name, callback_group_ );
// Make sure the server is actually there before continuing
89 RCLCPP_DEBUG( node_->get_logger( ), "Waiting for \"%s\" action server", action_name.c_str( ) );
90 action_client_->wait_for_action_server( );
}
/**
* @brief Any subclass of BtCancelActionNode that accepts parameters must provide a
* providedPorts method and call providedBasicPorts in it.
* @param addition Additional ports to add to BT port list
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
99 static BT::PortsList providedBasicPorts( BT::PortsList addition )
{
BT::PortsList basic = {
BT::InputPort<std::string>( "server_name", "Action server name" ),
BT::InputPort<std::chrono::milliseconds>( "server_timeout" )
};
basic.insert( addition.begin( ), addition.end( ) );
return basic;
}
110 void halt( )
{
}
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
118 static BT::PortsList providedPorts( )
{
return providedBasicPorts( {} );
}
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
127 BT::NodeStatus tick( ) override
{
// setting the status to RUNNING to notify the BT Loggers ( if any )
setStatus( BT::NodeStatus::RUNNING );
// Cancel all the goals specified before 10ms from current time
// to avoid async communication error
rclcpp::Time goal_expiry_time = node_->now( ) - std::chrono::milliseconds( 10 );
auto future_cancel = action_client_->async_cancel_goals_before( goal_expiry_time );
if ( callback_group_executor_.spin_until_future_complete( future_cancel, server_timeout_ ) !=
rclcpp::FutureReturnCode::SUCCESS )
{
RCLCPP_ERROR(
node_->get_logger( ),
"Failed to cancel the action server for %s", action_name_.c_str( ) );
return BT::NodeStatus::FAILURE;
}
return BT::NodeStatus::SUCCESS;
}
protected:
std::string action_name_;
typename std::shared_ptr<rclcpp_action::Client<ActionT>> action_client_;
// The node that will be used for any ROS operations
rclcpp::Node::SharedPtr node_;
rclcpp::CallbackGroup::SharedPtr callback_group_;
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
// The timeout value while waiting for response from a server when a
// new action goal is canceled
std::chrono::milliseconds server_timeout_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__BT_CANCEL_ACTION_NODE_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__BT_CONVERSIONS_HPP_
#define NAV2_BEHAVIOR_TREE__BT_CONVERSIONS_HPP_
#include <string>
#include "rclcpp/time.hpp"
#include "behaviortree_cpp_v3/behavior_tree.h"
#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
namespace BT
{
// The follow templates are required when using these types as parameters
// in our BT XML files. They parse the strings in the XML into their corresponding
// data type.
/**
* @brief Parse XML string to geometry_msgs::msg::Point
* @param key XML string
* @return geometry_msgs::msg::Point
*/
template<>
39 inline geometry_msgs::msg::Point convertFromString( const StringView key )
{
// three real numbers separated by semicolons
auto parts = BT::splitString( key, ';' );
if ( parts.size( ) != 3 ) {
throw std::runtime_error( "invalid number of fields for point attribute )" );
} else {
geometry_msgs::msg::Point position;
position.x = BT::convertFromString<double>( parts[0] );
position.y = BT::convertFromString<double>( parts[1] );
position.z = BT::convertFromString<double>( parts[2] );
return position;
}
}
/**
* @brief Parse XML string to geometry_msgs::msg::Quaternion
* @param key XML string
* @return geometry_msgs::msg::Quaternion
*/
template<>
60 inline geometry_msgs::msg::Quaternion convertFromString( const StringView key )
{
// four real numbers separated by semicolons
auto parts = BT::splitString( key, ';' );
if ( parts.size( ) != 4 ) {
throw std::runtime_error( "invalid number of fields for orientation attribute )" );
} else {
geometry_msgs::msg::Quaternion orientation;
orientation.x = BT::convertFromString<double>( parts[0] );
orientation.y = BT::convertFromString<double>( parts[1] );
orientation.z = BT::convertFromString<double>( parts[2] );
orientation.w = BT::convertFromString<double>( parts[3] );
return orientation;
}
}
/**
* @brief Parse XML string to geometry_msgs::msg::PoseStamped
* @param key XML string
* @return geometry_msgs::msg::PoseStamped
*/
template<>
82 inline geometry_msgs::msg::PoseStamped convertFromString( const StringView key )
{
// 7 real numbers separated by semicolons
auto parts = BT::splitString( key, ';' );
if ( parts.size( ) != 9 ) {
throw std::runtime_error( "invalid number of fields for PoseStamped attribute )" );
} else {
geometry_msgs::msg::PoseStamped pose_stamped;
pose_stamped.header.stamp = rclcpp::Time( BT::convertFromString<int64_t>( parts[0] ) );
pose_stamped.header.frame_id = BT::convertFromString<std::string>( parts[1] );
pose_stamped.pose.position.x = BT::convertFromString<double>( parts[2] );
pose_stamped.pose.position.y = BT::convertFromString<double>( parts[3] );
pose_stamped.pose.position.z = BT::convertFromString<double>( parts[4] );
pose_stamped.pose.orientation.x = BT::convertFromString<double>( parts[5] );
pose_stamped.pose.orientation.y = BT::convertFromString<double>( parts[6] );
pose_stamped.pose.orientation.z = BT::convertFromString<double>( parts[7] );
pose_stamped.pose.orientation.w = BT::convertFromString<double>( parts[8] );
return pose_stamped;
}
}
/**
* @brief Parse XML string to std::chrono::milliseconds
* @param key XML string
* @return std::chrono::milliseconds
*/
template<>
109 inline std::chrono::milliseconds convertFromString<std::chrono::milliseconds>( const StringView key )
{
return std::chrono::milliseconds( std::stoul( key.data( ) ) );
}
} // namespace BT
#endif // NAV2_BEHAVIOR_TREE__BT_CONVERSIONS_HPP_
// Copyright ( c ) 2019 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__BT_SERVICE_NODE_HPP_
#define NAV2_BEHAVIOR_TREE__BT_SERVICE_NODE_HPP_
#include <string>
#include <memory>
#include "behaviortree_cpp_v3/action_node.h"
#include "nav2_util/node_utils.hpp"
#include "rclcpp/rclcpp.hpp"
#include "nav2_behavior_tree/bt_conversions.hpp"
namespace nav2_behavior_tree
{
/**
* @brief Abstract class representing a service based BT node
* @tparam ServiceT Type of service
*/
template<class ServiceT>
34 class BtServiceNode : public BT::ActionNodeBase
{
public:
/**
* @brief A nav2_behavior_tree::BtServiceNode constructor
* @param service_node_name Service name this node creates a client for
* @param conf BT node configuration
*/
42 BtServiceNode(
43 const std::string & service_node_name,
44 const BT::NodeConfiguration & conf )
: BT::ActionNodeBase( service_node_name, conf ), service_node_name_( service_node_name )
{
node_ = config( ).blackboard->template get<rclcpp::Node::SharedPtr>( "node" );
callback_group_ = node_->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive,
false );
callback_group_executor_.add_callback_group( callback_group_, node_->get_node_base_interface( ) );
// Get the required items from the blackboard
bt_loop_duration_ =
config( ).blackboard->template get<std::chrono::milliseconds>( "bt_loop_duration" );
server_timeout_ =
config( ).blackboard->template get<std::chrono::milliseconds>( "server_timeout" );
getInput<std::chrono::milliseconds>( "server_timeout", server_timeout_ );
// Now that we have node_ to use, create the service client for this BT service
getInput( "service_name", service_name_ );
service_client_ = node_->create_client<ServiceT>(
service_name_,
rmw_qos_profile_services_default,
callback_group_ );
// Make a request for the service without parameter
request_ = std::make_shared<typename ServiceT::Request>( );
// Make sure the server is actually there before continuing
RCLCPP_DEBUG(
node_->get_logger( ), "Waiting for \"%s\" service",
service_name_.c_str( ) );
service_client_->wait_for_service( );
RCLCPP_DEBUG(
node_->get_logger( ), "\"%s\" BtServiceNode initialized",
service_node_name_.c_str( ) );
}
BtServiceNode( ) = delete;
virtual ~BtServiceNode( )
{
}
/**
* @brief Any subclass of BtServiceNode that accepts parameters must provide a
* providedPorts method and call providedBasicPorts in it.
* @param addition Additional ports to add to BT port list
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
static BT::PortsList providedBasicPorts( BT::PortsList addition )
{
BT::PortsList basic = {
BT::InputPort<std::string>( "service_name", "please_set_service_name_in_BT_Node" ),
BT::InputPort<std::chrono::milliseconds>( "server_timeout" )
};
99 basic.insert( addition.begin( ), addition.end( ) );
return basic;
}
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
108 static BT::PortsList providedPorts( )
{
return providedBasicPorts( {} );
}
/**
* @brief The main override required by a BT service
* @return BT::NodeStatus Status of tick execution
*/
117 BT::NodeStatus tick( ) override
{
if ( !request_sent_ ) {
on_tick( );
future_result_ = service_client_->async_send_request( request_ ).share( );
sent_time_ = node_->now( );
request_sent_ = true;
}
return check_future( );
}
/**
* @brief The other ( optional ) override required by a BT service.
*/
131 void halt( ) override
{
request_sent_ = false;
setStatus( BT::NodeStatus::IDLE );
}
/**
* @brief Function to perform some user-defined operation on tick
* Fill in service request with information if necessary
*/
virtual void on_tick( )
{
}
/**
* @brief Function to perform some user-defined operation upon successful
* completion of the service. Could put a value on the blackboard.
* @param response can be used to get the result of the service call in the BT Node.
* @return BT::NodeStatus Returns SUCCESS by default, user may override to return another value
*/
virtual BT::NodeStatus on_completion( std::shared_ptr<typename ServiceT::Response>/*response*/ )
{
return BT::NodeStatus::SUCCESS;
}
/**
* @brief Check the future and decide the status of BT
* @return BT::NodeStatus SUCCESS if future complete before timeout, FAILURE otherwise
*/
virtual BT::NodeStatus check_future( )
{
auto elapsed = ( node_->now( ) - sent_time_ ).to_chrono<std::chrono::milliseconds>( );
auto remaining = server_timeout_ - elapsed;
if ( remaining > std::chrono::milliseconds( 0 ) ) {
auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining;
rclcpp::FutureReturnCode rc;
rc = callback_group_executor_.spin_until_future_complete( future_result_, server_timeout_ );
if ( rc == rclcpp::FutureReturnCode::SUCCESS ) {
request_sent_ = false;
BT::NodeStatus status = on_completion( future_result_.get( ) );
return status;
}
if ( rc == rclcpp::FutureReturnCode::TIMEOUT ) {
on_wait_for_result( );
elapsed = ( node_->now( ) - sent_time_ ).to_chrono<std::chrono::milliseconds>( );
if ( elapsed < server_timeout_ ) {
return BT::NodeStatus::RUNNING;
}
}
}
RCLCPP_WARN(
node_->get_logger( ),
"Node timed out while executing service call to %s.", service_name_.c_str( ) );
request_sent_ = false;
return BT::NodeStatus::FAILURE;
}
/**
* @brief Function to perform some user-defined operation after a timeout waiting
* for a result that hasn't been received yet
*/
virtual void on_wait_for_result( )
{
}
protected:
/**
* @brief Function to increment recovery count on blackboard if this node wraps a recovery
*/
void increment_recovery_count( )
{
int recovery_count = 0;
config( ).blackboard->template get<int>( "number_recoveries", recovery_count ); // NOLINT
recovery_count += 1;
config( ).blackboard->template set<int>( "number_recoveries", recovery_count ); // NOLINT
}
std::string service_name_, service_node_name_;
typename std::shared_ptr<rclcpp::Client<ServiceT>> service_client_;
std::shared_ptr<typename ServiceT::Request> request_;
// The node that will be used for any ROS operations
rclcpp::Node::SharedPtr node_;
rclcpp::CallbackGroup::SharedPtr callback_group_;
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
// The timeout value while to use in the tick loop while waiting for
// a result from the server
std::chrono::milliseconds server_timeout_;
// The timeout value for BT loop execution
std::chrono::milliseconds bt_loop_duration_;
// To track the server response when a new request is sent
std::shared_future<typename ServiceT::Response::SharedPtr> future_result_;
bool request_sent_{false};
rclcpp::Time sent_time_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__BT_SERVICE_NODE_HPP_
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_ACTION_HPP_
#include <string>
#include "nav2_behavior_tree/bt_action_node.hpp"
#include "nav2_msgs/action/back_up.hpp"
namespace nav2_behavior_tree
{
/**
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::BackUp
*/
29 class BackUpAction : public BtActionNode<nav2_msgs::action::BackUp>
{
public:
/**
* @brief A constructor for nav2_behavior_tree::BackUpAction
* @param xml_tag_name Name for the XML tag for this node
* @param action_name Action name this node creates a client for
* @param conf BT node configuration
*/
38 BackUpAction(
39 const std::string & xml_tag_name,
40 const std::string & action_name,
41 const BT::NodeConfiguration & conf );
/**
* @brief Function to perform some user-defined operation on tick
*/
void on_tick( ) override;
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
static BT::PortsList providedPorts( )
{
return providedBasicPorts(
{
BT::InputPort<double>( "backup_dist", 0.15, "Distance to backup" ),
BT::InputPort<double>( "backup_speed", 0.025, "Speed at which to backup" ),
BT::InputPort<double>( "time_allowance", 10.0, "Allowed time for reversing" )
} );
}
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_ACTION_HPP_
1 // Copyright ( c ) 2022 Neobotix GmbH
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_CANCEL_NODE_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_CANCEL_NODE_HPP_
#include <memory>
#include <string>
#include "nav2_msgs/action/back_up.hpp"
#include "nav2_behavior_tree/bt_cancel_action_node.hpp"
namespace nav2_behavior_tree
{
/**
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::BackUp
*/
31 class BackUpCancel : public BtCancelActionNode<nav2_msgs::action::BackUp>
{
public:
/**
* @brief A constructor for nav2_behavior_tree::BackUpAction
* @param xml_tag_name Name for the XML tag for this node
* @param action_name Action name this node creates a client for
* @param conf BT node configuration
*/
40 BackUpCancel(
41 const std::string & xml_tag_name,
42 const std::string & action_name,
43 const BT::NodeConfiguration & conf );
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
49 static BT::PortsList providedPorts( )
{
return providedBasicPorts(
{
} );
}
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__BACK_UP_CANCEL_NODE_HPP_
// Copyright ( c ) 2019 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CLEAR_COSTMAP_SERVICE_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CLEAR_COSTMAP_SERVICE_HPP_
#include <string>
#include "nav2_behavior_tree/bt_service_node.hpp"
#include "nav2_msgs/srv/clear_entire_costmap.hpp"
#include "nav2_msgs/srv/clear_costmap_around_robot.hpp"
#include "nav2_msgs/srv/clear_costmap_except_region.hpp"
namespace nav2_behavior_tree
{
/**
* @brief A nav2_behavior_tree::BtServiceNode class that wraps nav2_msgs::srv::ClearEntireCostmap
*/
31 class ClearEntireCostmapService : public BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>
{
public:
/**
* @brief A constructor for nav2_behavior_tree::ClearEntireCostmapService
* @param service_node_name Service name this node creates a client for
* @param conf BT node configuration
*/
39 ClearEntireCostmapService(
40 const std::string & service_node_name,
41 const BT::NodeConfiguration & conf );
/**
* @brief The main override required by a BT service
* @return BT::NodeStatus Status of tick execution
*/
void on_tick( ) override;
};
/**
* @brief A nav2_behavior_tree::BtServiceNode class that
* wraps nav2_msgs::srv::ClearCostmapExceptRegion
*/
54 class ClearCostmapExceptRegionService
55 : public BtServiceNode<nav2_msgs::srv::ClearCostmapExceptRegion>
{
public:
/**
* @brief A constructor for nav2_behavior_tree::ClearCostmapExceptRegionService
* @param service_node_name Service name this node creates a client for
* @param conf BT node configuration
*/
63 ClearCostmapExceptRegionService(
64 const std::string & service_node_name,
65 const BT::NodeConfiguration & conf );
/**
* @brief The main override required by a BT service
* @return BT::NodeStatus Status of tick execution
*/
void on_tick( ) override;
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
static BT::PortsList providedPorts( )
{
return providedBasicPorts(
{
BT::InputPort<double>(
"reset_distance", 1,
"Distance from the robot above which obstacles are cleared" )
} );
}
};
/**
* @brief A nav2_behavior_tree::BtServiceNode class that
* wraps nav2_msgs::srv::ClearCostmapAroundRobot
*/
class ClearCostmapAroundRobotService : public BtServiceNode<nav2_msgs::srv::ClearCostmapAroundRobot>
{
public:
/**
* @brief A constructor for nav2_behavior_tree::ClearCostmapAroundRobotService
* @param service_node_name Service name this node creates a client for
* @param conf BT node configuration
*/
ClearCostmapAroundRobotService(
const std::string & service_node_name,
const BT::NodeConfiguration & conf );
/**
* @brief The main override required by a BT service
* @return BT::NodeStatus Status of tick execution
*/
void on_tick( ) override;
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
static BT::PortsList providedPorts( )
{
return providedBasicPorts(
{
BT::InputPort<double>(
"reset_distance", 1,
"Distance from the robot under which obstacles are cleared" )
} );
}
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CLEAR_COSTMAP_SERVICE_HPP_
// Copyright ( c ) 2021 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_THROUGH_POSES_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_THROUGH_POSES_ACTION_HPP_
#include <string>
#include <vector>
#include "nav2_msgs/action/compute_path_through_poses.hpp"
#include "nav_msgs/msg/path.h"
#include "nav2_behavior_tree/bt_action_node.hpp"
namespace nav2_behavior_tree
{
/**
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::ComputePathThroughPoses
*/
32 class ComputePathThroughPosesAction
33 : public BtActionNode<nav2_msgs::action::ComputePathThroughPoses>
{
public:
/**
* @brief A constructor for nav2_behavior_tree::ComputePathThroughPosesAction
* @param xml_tag_name Name for the XML tag for this node
* @param action_name Action name this node creates a client for
* @param conf BT node configuration
*/
42 ComputePathThroughPosesAction(
43 const std::string & xml_tag_name,
44 const std::string & action_name,
45 const BT::NodeConfiguration & conf );
/**
* @brief Function to perform some user-defined operation on tick
*/
void on_tick( ) override;
/**
* @brief Function to perform some user-defined operation upon successful completion of the action
*/
BT::NodeStatus on_success( ) override;
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
static BT::PortsList providedPorts( )
{
return providedBasicPorts(
{
BT::OutputPort<nav_msgs::msg::Path>( "path", "Path created by ComputePathThroughPoses node" ),
BT::InputPort<std::vector<geometry_msgs::msg::PoseStamped>>(
"goals",
"Destinations to plan through" ),
BT::InputPort<geometry_msgs::msg::PoseStamped>(
"start", "Start pose of the path if overriding current robot pose" ),
BT::InputPort<std::string>(
"planner_id", "",
"Mapped name to the planner plugin type to use" ),
} );
}
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_THROUGH_POSES_ACTION_HPP_
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_TO_POSE_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_TO_POSE_ACTION_HPP_
#include <string>
#include "nav2_msgs/action/compute_path_to_pose.hpp"
#include "nav_msgs/msg/path.h"
#include "nav2_behavior_tree/bt_action_node.hpp"
namespace nav2_behavior_tree
{
/**
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::ComputePathToPose
*/
30 class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePathToPose>
{
public:
/**
* @brief A constructor for nav2_behavior_tree::ComputePathToPoseAction
* @param xml_tag_name Name for the XML tag for this node
* @param action_name Action name this node creates a client for
* @param conf BT node configuration
*/
39 ComputePathToPoseAction(
40 const std::string & xml_tag_name,
41 const std::string & action_name,
42 const BT::NodeConfiguration & conf );
/**
* @brief Function to perform some user-defined operation on tick
*/
void on_tick( ) override;
/**
* @brief Function to perform some user-defined operation upon successful completion of the action
*/
BT::NodeStatus on_success( ) override;
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
static BT::PortsList providedPorts( )
{
return providedBasicPorts(
{
BT::OutputPort<nav_msgs::msg::Path>( "path", "Path created by ComputePathToPose node" ),
BT::InputPort<geometry_msgs::msg::PoseStamped>( "goal", "Destination to plan to" ),
BT::InputPort<geometry_msgs::msg::PoseStamped>(
"start", "Start pose of the path if overriding current robot pose" ),
BT::InputPort<std::string>(
"planner_id", "",
"Mapped name to the planner plugin type to use" ),
} );
}
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__COMPUTE_PATH_TO_POSE_ACTION_HPP_
1 // Copyright ( c ) 2022 Neobotix GmbH
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_CANCEL_NODE_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_CANCEL_NODE_HPP_
#include <memory>
#include <string>
#include "nav2_msgs/action/follow_path.hpp"
#include "nav2_behavior_tree/bt_cancel_action_node.hpp"
namespace nav2_behavior_tree
{
/**
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::FollowPath
*/
31 class ControllerCancel : public BtCancelActionNode<nav2_msgs::action::FollowPath>
{
public:
/**
* @brief A constructor for nav2_behavior_tree::FollowPathAction
* @param xml_tag_name Name for the XML tag for this node
* @param action_name Action name this node creates a client for
* @param conf BT node configuration
*/
40 ControllerCancel(
41 const std::string & xml_tag_name,
42 const std::string & action_name,
43 const BT::NodeConfiguration & conf );
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
49 static BT::PortsList providedPorts( )
{
return providedBasicPorts(
{
} );
}
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_CANCEL_NODE_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Pablo Iñigo Blasco
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_SELECTOR_NODE_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_SELECTOR_NODE_HPP_
#include <memory>
#include <string>
#include "std_msgs/msg/string.hpp"
#include "behaviortree_cpp_v3/action_node.h"
#include "rclcpp/rclcpp.hpp"
namespace nav2_behavior_tree
{
/**
* @brief The ControllerSelector behavior is used to switch the controller
* that will be used by the controller server. It subscribes to a topic "controller_selector"
* to get the decision about what controller must be used. It is usually used before of
* the FollowPath. The selected_controller output port is passed to controller_id
* input port of the FollowPath
*/
38 class ControllerSelector : public BT::SyncActionNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::ControllerSelector
*
* @param xml_tag_name Name for the XML tag for this node
* @param conf BT node configuration
*/
47 ControllerSelector(
48 const std::string & xml_tag_name,
49 const BT::NodeConfiguration & conf );
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
55 static BT::PortsList providedPorts( )
{
return {
BT::InputPort<std::string>(
"default_controller",
"the default controller to use if there is not any external topic message received." ),
BT::InputPort<std::string>(
"topic_name",
"controller_selector",
"the input topic name to select the controller" ),
BT::OutputPort<std::string>(
"selected_controller",
"Selected controller by subscription" )
};
}
private:
/**
* @brief Function to perform some user-defined operation on tick
*/
BT::NodeStatus tick( ) override;
/**
* @brief callback function for the controller_selector topic
*
* @param msg the message with the id of the controller_selector
*/
void callbackControllerSelect( const std_msgs::msg::String::SharedPtr msg );
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr controller_selector_sub_;
std::string last_selected_controller_;
rclcpp::Node::SharedPtr node_;
rclcpp::CallbackGroup::SharedPtr callback_group_;
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
std::string topic_name_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__CONTROLLER_SELECTOR_NODE_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_ACTION_HPP_
#include <string>
#include "nav2_behavior_tree/bt_action_node.hpp"
#include "nav2_msgs/action/drive_on_heading.hpp"
namespace nav2_behavior_tree
{
/**
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::DriveOnHeading
*/
29 class DriveOnHeadingAction : public BtActionNode<nav2_msgs::action::DriveOnHeading>
{
public:
/**
* @brief A constructor for nav2_behavior_tree::DriveOnHeadingAction
* @param xml_tag_name Name for the XML tag for this node
* @param action_name Action name this node creates a client for
* @param conf BT node configuration
*/
38 DriveOnHeadingAction(
39 const std::string & xml_tag_name,
40 const std::string & action_name,
41 const BT::NodeConfiguration & conf );
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
47 static BT::PortsList providedPorts( )
{
return providedBasicPorts(
{
BT::InputPort<double>( "dist_to_travel", 0.15, "Distance to travel" ),
BT::InputPort<double>( "speed", 0.025, "Speed at which to travel" ),
BT::InputPort<double>( "time_allowance", 10.0, "Allowed time for driving on heading" )
} );
}
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_ACTION_HPP_
1 // Copyright ( c ) 2022 Neobotix GmbH
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_CANCEL_NODE_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_CANCEL_NODE_HPP_
#include <memory>
#include <string>
#include "nav2_msgs/action/drive_on_heading.hpp"
#include "nav2_behavior_tree/bt_cancel_action_node.hpp"
namespace nav2_behavior_tree
{
/**
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::DriveOnHeading
*/
31 class DriveOnHeadingCancel : public BtCancelActionNode<nav2_msgs::action::DriveOnHeading>
{
public:
/**
* @brief A constructor for nav2_behavior_tree::DriveOnHeadingCancel
* @param xml_tag_name Name for the XML tag for this node
* @param action_name Action name this node creates a client for
* @param conf BT node configuration
*/
40 DriveOnHeadingCancel(
41 const std::string & xml_tag_name,
42 const std::string & action_name,
43 const BT::NodeConfiguration & conf );
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
49 static BT::PortsList providedPorts( )
{
return providedBasicPorts(
{
} );
}
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__DRIVE_ON_HEADING_CANCEL_NODE_HPP_
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_PATH_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_PATH_ACTION_HPP_
#include <string>
#include <memory>
#include "nav2_msgs/action/follow_path.hpp"
#include "nav2_behavior_tree/bt_action_node.hpp"
namespace nav2_behavior_tree
{
/**
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::FollowPath
*/
30 class FollowPathAction : public BtActionNode<nav2_msgs::action::FollowPath>
{
public:
/**
* @brief A constructor for nav2_behavior_tree::FollowPathAction
* @param xml_tag_name Name for the XML tag for this node
* @param action_name Action name this node creates a client for
* @param conf BT node configuration
*/
39 FollowPathAction(
40 const std::string & xml_tag_name,
41 const std::string & action_name,
42 const BT::NodeConfiguration & conf );
/**
* @brief Function to perform some user-defined operation on tick
*/
void on_tick( ) override;
/**
* @brief Function to perform some user-defined operation after a timeout
* waiting for a result that hasn't been received yet
* @param feedback shared_ptr to latest feedback message
*/
void on_wait_for_result(
std::shared_ptr<const nav2_msgs::action::FollowPath::Feedback> feedback ) override;
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
static BT::PortsList providedPorts( )
{
return providedBasicPorts(
{
BT::InputPort<nav_msgs::msg::Path>( "path", "Path to follow" ),
BT::InputPort<std::string>( "controller_id", "" ),
BT::InputPort<std::string>( "goal_checker_id", "" ),
} );
}
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__FOLLOW_PATH_ACTION_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Pablo Iñigo Blasco
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GOAL_CHECKER_SELECTOR_NODE_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GOAL_CHECKER_SELECTOR_NODE_HPP_
#include <memory>
#include <string>
#include "std_msgs/msg/string.hpp"
#include "behaviortree_cpp_v3/action_node.h"
#include "rclcpp/rclcpp.hpp"
namespace nav2_behavior_tree
{
/**
* @brief The GoalCheckerSelector behavior is used to switch the goal checker
* of the controller server. It subscribes to a topic "goal_checker_selector"
* to get the decision about what goal_checker must be used. It is usually used before of
* the FollowPath. The selected_goal_checker output port is passed to goal_checker_id
* input port of the FollowPath
*/
38 class GoalCheckerSelector : public BT::SyncActionNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::GoalCheckerSelector
*
* @param xml_tag_name Name for the XML tag for this node
* @param conf BT node configuration
*/
47 GoalCheckerSelector(
48 const std::string & xml_tag_name,
49 const BT::NodeConfiguration & conf );
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
55 static BT::PortsList providedPorts( )
{
return {
BT::InputPort<std::string>(
"default_goal_checker",
"the default goal_checker to use if there is not any external topic message received." ),
BT::InputPort<std::string>(
"topic_name",
"goal_checker_selector",
"the input topic name to select the goal_checker" ),
BT::OutputPort<std::string>(
"selected_goal_checker",
"Selected goal_checker by subscription" )
};
}
private:
/**
* @brief Function to perform some user-defined operation on tick
*/
BT::NodeStatus tick( ) override;
/**
* @brief callback function for the goal_checker_selector topic
*
* @param msg the message with the id of the goal_checker_selector
*/
void callbackGoalCheckerSelect( const std_msgs::msg::String::SharedPtr msg );
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr goal_checker_selector_sub_;
std::string last_selected_goal_checker_;
rclcpp::Node::SharedPtr node_;
std::string topic_name_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GOAL_CHECKER_SELECTOR_NODE_HPP_
// Copyright ( c ) 2021 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_THROUGH_POSES_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_THROUGH_POSES_ACTION_HPP_
#include <string>
#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "nav2_msgs/action/navigate_through_poses.hpp"
#include "nav2_behavior_tree/bt_action_node.hpp"
namespace nav2_behavior_tree
{
/**
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::NavigateThroughPoses
*/
31 class NavigateThroughPosesAction : public BtActionNode<nav2_msgs::action::NavigateThroughPoses>
{
public:
/**
* @brief A constructor for nav2_behavior_tree::NavigateThroughPosesAction
* @param xml_tag_name Name for the XML tag for this node
* @param action_name Action name this node creates a client for
* @param conf BT node configuration
*/
40 NavigateThroughPosesAction(
41 const std::string & xml_tag_name,
42 const std::string & action_name,
43 const BT::NodeConfiguration & conf );
/**
* @brief Function to perform some user-defined operation on tick
*/
void on_tick( ) override;
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
static BT::PortsList providedPorts( )
{
return providedBasicPorts(
{
BT::InputPort<geometry_msgs::msg::PoseStamped>( "goals", "Destinations to plan through" ),
BT::InputPort<std::string>( "behavior_tree", "Behavior tree to run" ),
} );
}
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_THROUGH_POSES_ACTION_HPP_
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_TO_POSE_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_TO_POSE_ACTION_HPP_
#include <string>
#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "nav2_behavior_tree/bt_action_node.hpp"
namespace nav2_behavior_tree
{
/**
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::NavigateToPose
*/
31 class NavigateToPoseAction : public BtActionNode<nav2_msgs::action::NavigateToPose>
{
public:
/**
* @brief A constructor for nav2_behavior_tree::NavigateToPoseAction
* @param xml_tag_name Name for the XML tag for this node
* @param action_name Action name this node creates a client for
* @param conf BT node configuration
*/
40 NavigateToPoseAction(
41 const std::string & xml_tag_name,
42 const std::string & action_name,
43 const BT::NodeConfiguration & conf );
/**
* @brief Function to perform some user-defined operation on tick
*/
void on_tick( ) override;
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
static BT::PortsList providedPorts( )
{
return providedBasicPorts(
{
BT::InputPort<geometry_msgs::msg::PoseStamped>( "goal", "Destination to plan to" ),
BT::InputPort<std::string>( "behavior_tree", "Behavior tree to run" ),
} );
}
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAVIGATE_TO_POSE_ACTION_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Pablo Iñigo Blasco
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PLANNER_SELECTOR_NODE_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PLANNER_SELECTOR_NODE_HPP_
#include <memory>
#include <string>
#include "std_msgs/msg/string.hpp"
#include "behaviortree_cpp_v3/action_node.h"
#include "rclcpp/rclcpp.hpp"
namespace nav2_behavior_tree
{
/**
* @brief The PlannerSelector behavior is used to switch the planner
* that will be used by the planner server. It subscribes to a topic "planner_selector"
* to get the decision about what planner must be used. It is usually used before of
* the ComputePathToPoseAction. The selected_planner output port is passed to planner_id
* input port of the ComputePathToPoseAction
*/
38 class PlannerSelector : public BT::SyncActionNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::PlannerSelector
*
* @param xml_tag_name Name for the XML tag for this node
* @param conf BT node configuration
*/
47 PlannerSelector(
48 const std::string & xml_tag_name,
49 const BT::NodeConfiguration & conf );
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
55 static BT::PortsList providedPorts( )
{
return {
BT::InputPort<std::string>(
"default_planner",
"the default planner to use if there is not any external topic message received." ),
BT::InputPort<std::string>(
"topic_name",
"planner_selector",
"the input topic name to select the planner" ),
BT::OutputPort<std::string>(
"selected_planner",
"Selected planner by subscription" )
};
}
private:
/**
* @brief Function to perform some user-defined operation on tick
*/
BT::NodeStatus tick( ) override;
/**
* @brief callback function for the planner_selector topic
*
* @param msg the message with the id of the planner_selector
*/
void callbackPlannerSelect( const std_msgs::msg::String::SharedPtr msg );
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr planner_selector_sub_;
std::string last_selected_planner_;
rclcpp::Node::SharedPtr node_;
rclcpp::CallbackGroup::SharedPtr callback_group_;
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
std::string topic_name_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PLANNER_SELECTOR_NODE_HPP_
1 // Copyright ( c ) 2019 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_
#include <string>
#include "nav2_behavior_tree/bt_service_node.hpp"
#include "std_srvs/srv/empty.hpp"
namespace nav2_behavior_tree
{
/**
* @brief A nav2_behavior_tree::BtServiceNode class that wraps nav2_msgs::srv::Empty
*/
29 class ReinitializeGlobalLocalizationService : public BtServiceNode<std_srvs::srv::Empty>
{
public:
/**
* @brief A constructor for nav2_behavior_tree::ReinitializeGlobalLocalizationService
* @param service_node_name Service name this node creates a client for
* @param conf BT node configuration
*/
37 ReinitializeGlobalLocalizationService(
38 const std::string & service_node_name,
39 const BT::NodeConfiguration & conf );
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REINITIALIZE_GLOBAL_LOCALIZATION_SERVICE_HPP_
// Copyright ( c ) 2021 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REMOVE_PASSED_GOALS_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REMOVE_PASSED_GOALS_ACTION_HPP_
#include <vector>
#include <memory>
#include <string>
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_util/robot_utils.hpp"
#include "behaviortree_cpp_v3/action_node.h"
namespace nav2_behavior_tree
{
30 class RemovePassedGoals : public BT::ActionNodeBase
{
public:
typedef std::vector<geometry_msgs::msg::PoseStamped> Goals;
35 RemovePassedGoals(
36 const std::string & xml_tag_name,
37 const BT::NodeConfiguration & conf );
40 static BT::PortsList providedPorts( )
{
return {
BT::InputPort<Goals>( "input_goals", "Original goals to remove viapoints from" ),
BT::OutputPort<Goals>( "output_goals", "Goals with passed viapoints removed" ),
BT::InputPort<double>( "radius", 0.5, "radius to goal for it to be considered for removal" ),
BT::InputPort<std::string>( "global_frame", std::string( "map" ), "Global frame" ),
BT::InputPort<std::string>( "robot_base_frame", std::string( "base_link" ), "Robot base frame" ),
};
}
private:
void halt( ) override {}
53 BT::NodeStatus tick( ) override;
double viapoint_achieved_radius_;
std::string robot_base_frame_, global_frame_;
double transform_tolerance_;
std::shared_ptr<tf2_ros::Buffer> tf_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__REMOVE_PASSED_GOALS_ACTION_HPP_
// Copyright ( c ) 2021 RoboTech Vision
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTH_PATH_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTH_PATH_ACTION_HPP_
#include <string>
#include "nav2_msgs/action/smooth_path.hpp"
#include "nav_msgs/msg/path.h"
#include "nav2_behavior_tree/bt_action_node.hpp"
namespace nav2_behavior_tree
{
/**
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::SmoothPath
*/
31 class SmoothPathAction : public nav2_behavior_tree::BtActionNode<nav2_msgs::action::SmoothPath>
{
public:
/**
* @brief A constructor for nav2_behavior_tree::SmoothPathAction
* @param xml_tag_name Name for the XML tag for this node
* @param action_name Action name this node creates a client for
* @param conf BT node configuration
*/
40 SmoothPathAction(
41 const std::string & xml_tag_name,
42 const std::string & action_name,
43 const BT::NodeConfiguration & conf );
/**
* @brief Function to perform some user-defined operation on tick
*/
void on_tick( ) override;
/**
* @brief Function to perform some user-defined operation upon successful completion of the action
*/
BT::NodeStatus on_success( ) override;
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
static BT::PortsList providedPorts( )
{
return providedBasicPorts(
{
BT::OutputPort<nav_msgs::msg::Path>(
"smoothed_path",
"Path smoothed by SmootherServer node" ),
BT::OutputPort<double>( "smoothing_duration", "Time taken to smooth path" ),
BT::OutputPort<bool>(
"was_completed", "True if smoothing was not interrupted by time limit" ),
BT::InputPort<nav_msgs::msg::Path>( "unsmoothed_path", "Path to be smoothed" ),
BT::InputPort<double>( "max_smoothing_duration", 3.0, "Maximum smoothing duration" ),
BT::InputPort<bool>(
"check_for_collisions", false,
"If true collision check will be performed after smoothing" ),
BT::InputPort<std::string>( "smoother_id", "" ),
} );
}
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTH_PATH_ACTION_HPP_
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SPIN_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SPIN_ACTION_HPP_
#include <string>
#include "nav2_behavior_tree/bt_action_node.hpp"
#include "nav2_msgs/action/spin.hpp"
namespace nav2_behavior_tree
{
/**
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::Spin
*/
29 class SpinAction : public BtActionNode<nav2_msgs::action::Spin>
{
public:
/**
* @brief A constructor for nav2_behavior_tree::SpinAction
* @param xml_tag_name Name for the XML tag for this node
* @param action_name Action name this node creates a client for
* @param conf BT node configuration
*/
38 SpinAction(
39 const std::string & xml_tag_name,
40 const std::string & action_name,
41 const BT::NodeConfiguration & conf );
/**
* @brief Function to perform some user-defined operation on tick
*/
void on_tick( ) override;
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
static BT::PortsList providedPorts( )
{
return providedBasicPorts(
{
BT::InputPort<double>( "spin_dist", 1.57, "Spin distance" ),
BT::InputPort<double>( "time_allowance", 10.0, "Allowed time for spinning" ),
BT::InputPort<bool>( "is_recovery", true, "True if recovery" )
} );
}
private:
bool is_recovery_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SPIN_ACTION_HPP_
1 // Copyright ( c ) 2022 Neobotix GmbH
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SPIN_CANCEL_NODE_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SPIN_CANCEL_NODE_HPP_
#include <memory>
#include <string>
#include "nav2_msgs/action/spin.hpp"
#include "nav2_behavior_tree/bt_cancel_action_node.hpp"
namespace nav2_behavior_tree
{
/**
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::Wait
*/
31 class SpinCancel : public BtCancelActionNode<nav2_msgs::action::Spin>
{
public:
/**
* @brief A constructor for nav2_behavior_tree::WaitAction
* @param xml_tag_name Name for the XML tag for this node
* @param action_name Action name this node creates a client for
* @param conf BT node configuration
*/
40 SpinCancel(
41 const std::string & xml_tag_name,
42 const std::string & action_name,
43 const BT::NodeConfiguration & conf );
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
49 static BT::PortsList providedPorts( )
{
return providedBasicPorts(
{
} );
}
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SPIN_CANCEL_NODE_HPP_
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Francisco Martin Rico
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_ACTION_HPP_
#include <memory>
#include <string>
#include "nav_msgs/msg/path.hpp"
#include "behaviortree_cpp_v3/action_node.h"
namespace nav2_behavior_tree
{
/**
* @brief A BT::ActionNodeBase to shorten path by some distance
*/
32 class TruncatePath : public BT::ActionNodeBase
{
public:
/**
* @brief A nav2_behavior_tree::TruncatePath constructor
* @param xml_tag_name Name for the XML tag for this node
* @param conf BT node configuration
*/
40 TruncatePath(
41 const std::string & xml_tag_name,
42 const BT::NodeConfiguration & conf );
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
48 static BT::PortsList providedPorts( )
{
return {
BT::InputPort<nav_msgs::msg::Path>( "input_path", "Original Path" ),
BT::OutputPort<nav_msgs::msg::Path>( "output_path", "Path truncated to a certain distance" ),
BT::InputPort<double>( "distance", 1.0, "distance" ),
};
}
private:
/**
* @brief The other ( optional ) override required by a BT action.
*/
void halt( ) override {}
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
67 BT::NodeStatus tick( ) override;
double distance_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_ACTION_HPP_
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Francisco Martin Rico
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_LOCAL_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_LOCAL_ACTION_HPP_
#include <memory>
#include <string>
#include <limits>
#include "nav_msgs/msg/path.hpp"
#include "behaviortree_cpp_v3/action_node.h"
#include "tf2_ros/buffer.h"
namespace nav2_behavior_tree
{
/**
* @brief A BT::ActionNodeBase to shorten path to some distance around robot
*/
34 class TruncatePathLocal : public BT::ActionNodeBase
{
public:
/**
* @brief A nav2_behavior_tree::TruncatePathLocal constructor
* @param xml_tag_name Name for the XML tag for this node
* @param conf BT node configuration
*/
42 TruncatePathLocal(
43 const std::string & xml_tag_name,
44 const BT::NodeConfiguration & conf );
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
50 static BT::PortsList providedPorts( )
{
return {
BT::InputPort<nav_msgs::msg::Path>( "input_path", "Original Path" ),
BT::OutputPort<nav_msgs::msg::Path>(
"output_path", "Path truncated to a certain distance around robot" ),
BT::InputPort<double>(
"distance_forward", 8.0,
"Distance in forward direction" ),
BT::InputPort<double>(
"distance_backward", 4.0,
"Distance in backward direction" ),
BT::InputPort<std::string>(
"robot_frame", "base_link",
"Robot base frame id" ),
BT::InputPort<double>(
"transform_tolerance", 0.2,
"Transform lookup tolerance" ),
BT::InputPort<geometry_msgs::msg::PoseStamped>(
"pose", "Manually specified pose to be used"
"if overriding current robot pose" ),
BT::InputPort<double>(
"angular_distance_weight", 0.0,
"Weight of angular distance relative to positional distance when finding which path "
"pose is closest to robot. Not applicable on paths without orientations assigned" ),
BT::InputPort<double>(
"max_robot_pose_search_dist", std::numeric_limits<double>::infinity( ),
"Maximum forward integrated distance along the path ( starting from the last detected pose ) "
"to bound the search for the closest pose to the robot. When set to infinity ( default ), "
"whole path is searched every time" ),
};
}
private:
/**
* @brief The other ( optional ) override required by a BT action.
*/
void halt( ) override {}
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
93 BT::NodeStatus tick( ) override;
/**
* @brief Get either specified input pose or robot pose in path frame
* @param path_frame_id Frame ID of path
* @param pose Output pose
* @return True if succeeded
*/
101 bool getRobotPose( std::string path_frame_id, geometry_msgs::msg::PoseStamped & pose );
/**
* @brief A custom pose distance method which takes angular distance into account
* in addition to spatial distance ( to improve picking a correct pose near cusps and loops )
* @param pose1 Distance is computed between this pose and pose2
* @param pose2 Distance is computed between this pose and pose1
* @param angular_distance_weight Weight of angular distance relative to spatial distance
* ( 1.0 means that 1 radian of angular distance corresponds to 1 meter of spatial distance )
*/
111 static double poseDistance(
const geometry_msgs::msg::PoseStamped & pose1,
const geometry_msgs::msg::PoseStamped & pose2,
const double angular_distance_weight );
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
nav_msgs::msg::Path path_;
nav_msgs::msg::Path::_poses_type::iterator closest_pose_detection_begin_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__TRUNCATE_PATH_LOCAL_ACTION_HPP_
// Copyright ( c ) 2018 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__WAIT_ACTION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__WAIT_ACTION_HPP_
#include <string>
#include "nav2_behavior_tree/bt_action_node.hpp"
#include "nav2_msgs/action/wait.hpp"
namespace nav2_behavior_tree
{
/**
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::Wait
*/
29 class WaitAction : public BtActionNode<nav2_msgs::action::Wait>
{
public:
/**
* @brief A constructor for nav2_behavior_tree::WaitAction
* @param xml_tag_name Name for the XML tag for this node
* @param action_name Action name this node creates a client for
* @param conf BT node configuration
*/
38 WaitAction(
39 const std::string & xml_tag_name,
40 const std::string & action_name,
41 const BT::NodeConfiguration & conf );
/**
* @brief Function to perform some user-defined operation on tick
*/
void on_tick( ) override;
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
static BT::PortsList providedPorts( )
{
return providedBasicPorts(
{
BT::InputPort<int>( "wait_duration", 1, "Wait time" )
} );
}
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__WAIT_ACTION_HPP_
1 // Copyright ( c ) 2022 Neobotix GmbH
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__WAIT_CANCEL_NODE_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__WAIT_CANCEL_NODE_HPP_
#include <memory>
#include <string>
#include "nav2_msgs/action/wait.hpp"
#include "nav2_behavior_tree/bt_cancel_action_node.hpp"
namespace nav2_behavior_tree
{
/**
* @brief A nav2_behavior_tree::BtActionNode class that wraps nav2_msgs::action::Wait
*/
31 class WaitCancel : public BtCancelActionNode<nav2_msgs::action::Wait>
{
public:
/**
* @brief A constructor for nav2_behavior_tree::WaitAction
* @param xml_tag_name Name for the XML tag for this node
* @param action_name Action name this node creates a client for
* @param conf BT node configuration
*/
40 WaitCancel(
41 const std::string & xml_tag_name,
42 const std::string & action_name,
43 const BT::NodeConfiguration & conf );
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
49 static BT::PortsList providedPorts( )
{
return providedBasicPorts(
{
} );
}
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__WAIT_CANCEL_NODE_HPP_
// Copyright ( c ) 2020 Sarthak Mittal
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__DISTANCE_TRAVELED_CONDITION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__DISTANCE_TRAVELED_CONDITION_HPP_
#include <string>
#include <memory>
#include "behaviortree_cpp_v3/condition_node.h"
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "tf2_ros/buffer.h"
namespace nav2_behavior_tree
{
/**
* @brief A BT::ConditionNode that returns SUCCESS every time the robot
* travels a specified distance and FAILURE otherwise
*/
35 class DistanceTraveledCondition : public BT::ConditionNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::DistanceTraveledCondition
* @param condition_name Name for the XML tag for this node
* @param conf BT node configuration
*/
43 DistanceTraveledCondition(
44 const std::string & condition_name,
45 const BT::NodeConfiguration & conf );
DistanceTraveledCondition( ) = delete;
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
BT::NodeStatus tick( ) override;
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing node-specific ports
*/
static BT::PortsList providedPorts( )
{
return {
BT::InputPort<double>( "distance", 1.0, "Distance" ),
BT::InputPort<std::string>( "global_frame", std::string( "map" ), "Global frame" ),
BT::InputPort<std::string>( "robot_base_frame", std::string( "base_link" ), "Robot base frame" )
};
}
private:
rclcpp::Node::SharedPtr node_;
std::shared_ptr<tf2_ros::Buffer> tf_;
geometry_msgs::msg::PoseStamped start_pose_;
double distance_;
double transform_tolerance_;
std::string global_frame_;
std::string robot_base_frame_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__DISTANCE_TRAVELED_CONDITION_HPP_
// Copyright ( c ) 2021 Joshua Wallace
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GLOBALLY_UPDATED_GOAL_CONDITION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GLOBALLY_UPDATED_GOAL_CONDITION_HPP_
#include <string>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "behaviortree_cpp_v3/condition_node.h"
#include "geometry_msgs/msg/pose_stamped.hpp"
namespace nav2_behavior_tree
{
/**
* @brief A BT::ConditionNode that returns SUCCESS when goal is
* updated on the blackboard and FAILURE otherwise
*/
33 class GloballyUpdatedGoalCondition : public BT::ConditionNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::GloballyUpdatedGoalCondition
* @param condition_name Name for the XML tag for this node
* @param conf BT node configuration
*/
41 GloballyUpdatedGoalCondition(
42 const std::string & condition_name,
43 const BT::NodeConfiguration & conf );
GloballyUpdatedGoalCondition( ) = delete;
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
BT::NodeStatus tick( ) override;
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing node-specific ports
*/
static BT::PortsList providedPorts( )
{
return {};
}
private:
bool first_time;
rclcpp::Node::SharedPtr node_;
geometry_msgs::msg::PoseStamped goal_;
std::vector<geometry_msgs::msg::PoseStamped> goals_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GLOBALLY_UPDATED_GOAL_CONDITION_HPP_
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_REACHED_CONDITION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_REACHED_CONDITION_HPP_
#include <string>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "behaviortree_cpp_v3/condition_node.h"
#include "tf2_ros/buffer.h"
namespace nav2_behavior_tree
{
/**
* @brief A BT::ConditionNode that returns SUCCESS when a specified goal
* is reached and FAILURE otherwise
*/
32 class GoalReachedCondition : public BT::ConditionNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::GoalReachedCondition
* @param condition_name Name for the XML tag for this node
* @param conf BT node configuration
*/
40 GoalReachedCondition(
41 const std::string & condition_name,
42 const BT::NodeConfiguration & conf );
GoalReachedCondition( ) = delete;
/**
* @brief A destructor for nav2_behavior_tree::GoalReachedCondition
*/
~GoalReachedCondition( ) override;
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
BT::NodeStatus tick( ) override;
/**
* @brief Function to read parameters and initialize class variables
*/
void initialize( );
/**
* @brief Checks if the current robot pose lies within a given distance from the goal
* @return bool true when goal is reached, false otherwise
*/
bool isGoalReached( );
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing node-specific ports
*/
static BT::PortsList providedPorts( )
{
return {
BT::InputPort<geometry_msgs::msg::PoseStamped>( "goal", "Destination" ),
BT::InputPort<std::string>( "global_frame", std::string( "map" ), "Global frame" ),
BT::InputPort<std::string>( "robot_base_frame", std::string( "base_link" ), "Robot base frame" )
};
}
protected:
/**
* @brief Cleanup function
*/
85 void cleanup( )
{}
private:
rclcpp::Node::SharedPtr node_;
std::shared_ptr<tf2_ros::Buffer> tf_;
bool initialized_;
double goal_reached_tol_;
std::string global_frame_;
std::string robot_base_frame_;
double transform_tolerance_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_REACHED_CONDITION_HPP_
// Copyright ( c ) 2020 Aitor Miguel Blanco
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_UPDATED_CONDITION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_UPDATED_CONDITION_HPP_
#include <string>
#include <vector>
#include "behaviortree_cpp_v3/condition_node.h"
#include "geometry_msgs/msg/pose_stamped.hpp"
namespace nav2_behavior_tree
{
/**
* @brief A BT::ConditionNode that returns SUCCESS when goal is
* updated on the blackboard and FAILURE otherwise
*/
31 class GoalUpdatedCondition : public BT::ConditionNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::GoalUpdatedCondition
* @param condition_name Name for the XML tag for this node
* @param conf BT node configuration
*/
39 GoalUpdatedCondition(
40 const std::string & condition_name,
41 const BT::NodeConfiguration & conf );
GoalUpdatedCondition( ) = delete;
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
BT::NodeStatus tick( ) override;
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing node-specific ports
*/
static BT::PortsList providedPorts( )
{
return {};
}
private:
geometry_msgs::msg::PoseStamped goal_;
std::vector<geometry_msgs::msg::PoseStamped> goals_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__GOAL_UPDATED_CONDITION_HPP_
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_
#include "behaviortree_cpp_v3/behavior_tree.h"
namespace nav2_behavior_tree
{
/**
* @brief A BT::ConditionNode that returns SUCCESS if initial pose
* has been received and FAILURE otherwise
*/
26 BT::NodeStatus initialPoseReceived( BT::TreeNode & tree_node );
}
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_
// Copyright ( c ) 2020 Sarthak Mittal
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_LOW_CONDITION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_LOW_CONDITION_HPP_
#include <string>
#include <memory>
#include <mutex>
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/battery_state.hpp"
#include "behaviortree_cpp_v3/condition_node.h"
namespace nav2_behavior_tree
{
/**
* @brief A BT::ConditionNode that listens to a battery topic and
* returns SUCCESS when battery is low and FAILURE otherwise
*/
34 class IsBatteryLowCondition : public BT::ConditionNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::IsBatteryLowCondition
* @param condition_name Name for the XML tag for this node
* @param conf BT node configuration
*/
42 IsBatteryLowCondition(
43 const std::string & condition_name,
44 const BT::NodeConfiguration & conf );
IsBatteryLowCondition( ) = delete;
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
BT::NodeStatus tick( ) override;
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing node-specific ports
*/
static BT::PortsList providedPorts( )
{
return {
BT::InputPort<double>( "min_battery", "Minimum battery percentage/voltage" ),
BT::InputPort<std::string>(
"battery_topic", std::string( "/battery_status" ), "Battery topic" ),
BT::InputPort<bool>(
"is_voltage", false, "If true voltage will be used to check for low battery" ),
};
}
private:
/**
* @brief Callback function for battery topic
* @param msg Shared pointer to sensor_msgs::msg::BatteryState message
*/
74 void batteryCallback( sensor_msgs::msg::BatteryState::SharedPtr msg );
rclcpp::Node::SharedPtr node_;
rclcpp::CallbackGroup::SharedPtr callback_group_;
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
rclcpp::Subscription<sensor_msgs::msg::BatteryState>::SharedPtr battery_sub_;
std::string battery_topic_;
double min_battery_;
bool is_voltage_;
bool is_battery_low_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_LOW_CONDITION_HPP_
// Copyright ( c ) 2021 Joshua Wallace
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_PATH_VALID_CONDITION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_PATH_VALID_CONDITION_HPP_
#include <string>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "behaviortree_cpp_v3/condition_node.h"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_msgs/srv/is_path_valid.hpp"
namespace nav2_behavior_tree
{
/**
* @brief A BT::ConditionNode that returns SUCCESS when the IsPathValid
* service returns true and FAILURE otherwise
*/
33 class IsPathValidCondition : public BT::ConditionNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::IsPathValidCondition
* @param condition_name Name for the XML tag for this node
* @param conf BT node configuration
*/
41 IsPathValidCondition(
42 const std::string & condition_name,
43 const BT::NodeConfiguration & conf );
IsPathValidCondition( ) = delete;
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
BT::NodeStatus tick( ) override;
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing node-specific ports
*/
static BT::PortsList providedPorts( )
{
return {
BT::InputPort<nav_msgs::msg::Path>( "path", "Path to Check" ),
BT::InputPort<std::chrono::milliseconds>( "server_timeout" )
};
}
private:
rclcpp::Node::SharedPtr node_;
rclcpp::Client<nav2_msgs::srv::IsPathValid>::SharedPtr client_;
// The timeout value while waiting for a responce from the
// is path valid service
std::chrono::milliseconds server_timeout_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_PATH_VALID_CONDITION_HPP_
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STUCK_CONDITION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STUCK_CONDITION_HPP_
#include <string>
#include <atomic>
#include <deque>
#include "rclcpp/rclcpp.hpp"
#include "behaviortree_cpp_v3/condition_node.h"
#include "nav_msgs/msg/odometry.hpp"
namespace nav2_behavior_tree
{
/**
* @brief A BT::ConditionNode that tracks robot odometry and returns SUCCESS
* if robot is stuck somewhere and FAILURE otherwise
*/
33 class IsStuckCondition : public BT::ConditionNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::IsStuckCondition
* @param condition_name Name for the XML tag for this node
* @param conf BT node configuration
*/
41 IsStuckCondition(
42 const std::string & condition_name,
43 const BT::NodeConfiguration & conf );
IsStuckCondition( ) = delete;
/**
* @brief A destructor for nav2_behavior_tree::IsStuckCondition
*/
~IsStuckCondition( ) override;
/**
* @brief Callback function for odom topic
* @param msg Shared pointer to nav_msgs::msg::Odometry::SharedPtr message
*/
void onOdomReceived( const typename nav_msgs::msg::Odometry::SharedPtr msg );
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
BT::NodeStatus tick( ) override;
/**
* @brief Function to log status when robot is stuck/free
*/
void logStuck( const std::string & msg ) const;
/**
* @brief Function to approximate acceleration from the odom history
*/
void updateStates( );
/**
* @brief Detect if robot bumped into something by checking for abnormal deceleration
* @return bool true if robot is stuck, false otherwise
*/
bool isStuck( );
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing node-specific ports
*/
static BT::PortsList providedPorts( ) {return {};}
private:
// The node that will be used for any ROS operations
rclcpp::Node::SharedPtr node_;
rclcpp::CallbackGroup::SharedPtr callback_group_;
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
std::thread callback_group_executor_thread;
std::atomic<bool> is_stuck_;
// Listen to odometry
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
// Store history of odometry measurements
std::deque<nav_msgs::msg::Odometry> odom_history_;
std::deque<nav_msgs::msg::Odometry>::size_type odom_history_size_;
// Calculated states
double current_accel_;
// Robot specific paramters
double brake_accel_limit_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_STUCK_CONDITION_HPP_
// Copyright ( c ) 2022 Joshua Wallace
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__PATH_EXPIRING_TIMER_CONDITION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__PATH_EXPIRING_TIMER_CONDITION_HPP_
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "behaviortree_cpp_v3/condition_node.h"
#include "nav_msgs/msg/path.hpp"
namespace nav2_behavior_tree
{
/**
* @brief A BT::ConditionNode that returns SUCCESS every time a specified
* time period passes and FAILURE otherwise
*/
31 class PathExpiringTimerCondition : public BT::ConditionNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::PathExpiringTimerCondition
* @param condition_name Name for the XML tag for this node
* @param conf BT node configuration
*/
39 PathExpiringTimerCondition(
40 const std::string & condition_name,
41 const BT::NodeConfiguration & conf );
PathExpiringTimerCondition( ) = delete;
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
BT::NodeStatus tick( ) override;
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing node-specific ports
*/
static BT::PortsList providedPorts( )
{
return {
BT::InputPort<double>( "seconds", 1.0, "Seconds" ),
BT::InputPort<nav_msgs::msg::Path>( "path" )
};
}
private:
rclcpp::Node::SharedPtr node_;
rclcpp::Time start_;
nav_msgs::msg::Path prev_path_;
double period_;
bool first_time_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__PATH_EXPIRING_TIMER_CONDITION_HPP_
// Copyright ( c ) 2020 Sarthak Mittal
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TIME_EXPIRED_CONDITION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TIME_EXPIRED_CONDITION_HPP_
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "behaviortree_cpp_v3/condition_node.h"
namespace nav2_behavior_tree
{
/**
* @brief A BT::ConditionNode that returns SUCCESS every time a specified
* time period passes and FAILURE otherwise
*/
31 class TimeExpiredCondition : public BT::ConditionNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::TimeExpiredCondition
* @param condition_name Name for the XML tag for this node
* @param conf BT node configuration
*/
39 TimeExpiredCondition(
40 const std::string & condition_name,
41 const BT::NodeConfiguration & conf );
TimeExpiredCondition( ) = delete;
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
BT::NodeStatus tick( ) override;
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing node-specific ports
*/
static BT::PortsList providedPorts( )
{
return {
BT::InputPort<double>( "seconds", 1.0, "Seconds" )
};
}
private:
rclcpp::Node::SharedPtr node_;
rclcpp::Time start_;
double period_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TIME_EXPIRED_CONDITION_HPP_
// Copyright ( c ) 2020 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TRANSFORM_AVAILABLE_CONDITION_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TRANSFORM_AVAILABLE_CONDITION_HPP_
#include <string>
#include <atomic>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "behaviortree_cpp_v3/condition_node.h"
#include "tf2_ros/buffer.h"
namespace nav2_behavior_tree
{
/**
* @brief A BT::ConditionNode that returns SUCCESS if there is a valid transform
* between two specified frames and FAILURE otherwise
*/
33 class TransformAvailableCondition : public BT::ConditionNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::TransformAvailableCondition
* @param condition_name Name for the XML tag for this node
* @param conf BT node configuration
*/
41 TransformAvailableCondition(
42 const std::string & condition_name,
43 const BT::NodeConfiguration & conf );
TransformAvailableCondition( ) = delete;
/**
* @brief A destructor for nav2_behavior_tree::TransformAvailableCondition
*/
~TransformAvailableCondition( );
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
BT::NodeStatus tick( ) override;
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing node-specific ports
*/
static BT::PortsList providedPorts( )
{
return {
BT::InputPort<std::string>( "child", std::string( ), "Child frame for transform" ),
BT::InputPort<std::string>( "parent", std::string( ), "parent frame for transform" )
};
}
private:
rclcpp::Node::SharedPtr node_;
std::shared_ptr<tf2_ros::Buffer> tf_;
std::atomic<bool> was_found_;
std::string child_frame_;
std::string parent_frame_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__TRANSFORM_AVAILABLE_CONDITION_HPP_
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PIPELINE_SEQUENCE_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PIPELINE_SEQUENCE_HPP_
#include <string>
#include "behaviortree_cpp_v3/control_node.h"
#include "behaviortree_cpp_v3/bt_factory.h"
namespace nav2_behavior_tree
{
/** @brief Type of sequence node that re-ticks previous children when a child returns running
*
* Type of Control Node | Child Returns Failure | Child Returns Running
* ---------------------------------------------------------------------
* PipelineSequence | Restart | Tick All Previous Again
*
* Tick All Previous Again means every node up till this one will be reticked. Even
* if a previous node returns Running, the next node will be reticked.
*
* As an example, let's say this node has 3 children: A, B and C. At the start,
* they are all IDLE.
* | A | B | C |
* --------------------------------
* | IDLE | IDLE | IDLE |
* | RUNNING | IDLE | IDLE | - at first A gets ticked. Assume it returns RUNNING
* - PipelineSequence returns RUNNING and no other nodes are ticked.
* | SUCCESS | RUNNING | IDLE | - This time A returns SUCCESS so B gets ticked as well
* - PipelineSequence returns RUNNING and C is not ticked yet
* | RUNNING | SUCCESS | RUNNING | - A gets ticked and returns RUNNING, but since it had previously
* - returned SUCCESS, PipelineSequence continues on and ticks B.
* - Since B also returns SUCCESS, C gets ticked this time as well.
* | RUNNING | SUCCESS | SUCCESS | - A is still RUNNING, and B returns SUCCESS again. This time C
* - returned SUCCESS, ending the sequence. PipelineSequence
* - returns SUCCESS and halts A.
*
* If any children at any time had returned FAILURE. PipelineSequence would have returned FAILURE
* and halted all children, ending the sequence.
*
* Usage in XML: <PipelineSequence>
*/
55 class PipelineSequence : public BT::ControlNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::PipelineSequence
* @param name Name for the XML tag for this node
*/
62 explicit PipelineSequence( const std::string & name );
/**
* @brief A constructor for nav2_behavior_tree::PipelineSequence
* @param name Name for the XML tag for this node
* @param config BT node configuration
*/
69 PipelineSequence( const std::string & name, const BT::NodeConfiguration & config );
/**
* @brief The other ( optional ) override required by a BT action to reset node state
*/
void halt( ) override;
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
static BT::PortsList providedPorts( ) {return {};}
protected:
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
87 BT::NodeStatus tick( ) override;
std::size_t last_child_ticked_ = 0;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__PIPELINE_SEQUENCE_HPP_
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__RECOVERY_NODE_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__RECOVERY_NODE_HPP_
#include <string>
#include "behaviortree_cpp_v3/control_node.h"
namespace nav2_behavior_tree
{
/**
* @brief The RecoveryNode has only two children and returns SUCCESS if and only if the first child
* returns SUCCESS.
*
* - If the first child returns FAILURE, the second child will be executed. After that the first
* child is executed again if the second child returns SUCCESS.
*
* - If the first or second child returns RUNNING, this node returns RUNNING.
*
* - If the second child returns FAILURE, this control node will stop the loop and returns FAILURE.
*
*/
35 class RecoveryNode : public BT::ControlNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::RecoveryNode
* @param name Name for the XML tag for this node
* @param conf BT node configuration
*/
43 RecoveryNode(
44 const std::string & name,
45 const BT::NodeConfiguration & conf );
/**
* @brief A destructor for nav2_behavior_tree::RecoveryNode
*/
~RecoveryNode( ) override = default;
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
static BT::PortsList providedPorts( )
{
return {
BT::InputPort<int>( "number_of_retries", 1, "Number of retries" )
};
}
private:
unsigned int current_child_idx_;
unsigned int number_of_retries_;
unsigned int retry_count_;
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
72 BT::NodeStatus tick( ) override;
/**
* @brief The other ( optional ) override required by a BT action to reset node state
*/
77 void halt( ) override;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__RECOVERY_NODE_HPP_
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__ROUND_ROBIN_NODE_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__ROUND_ROBIN_NODE_HPP_
#include <string>
#include "behaviortree_cpp_v3/control_node.h"
#include "behaviortree_cpp_v3/bt_factory.h"
namespace nav2_behavior_tree
{
/** @brief Type of sequence node that ticks children in a round-robin fashion
*
* Type of Control Node | Child Returns Failure | Child Returns Running
* ---------------------------------------------------------------------
* RoundRobin | Tick Next Child | Return Running
*
* If the current child return failure, the next child is ticked and if the last child returns
* failure, the first child is ticked and the cycle continues until a child returns success
*
* As an example, let's say this node has 3 children: A, B and C. At the start,
* they are all IDLE.
* | A | B | C |
* --------------------------------
* | IDLE | IDLE | IDLE |
* | RUNNING | IDLE | IDLE | - at first A gets ticked. Assume it returns RUNNING
* - RoundRobin returns RUNNING and no other nodes are ticked.
* | FAILURE | RUNNING | IDLE | - A returns FAILURE so B gets ticked and returns RUNNING
* - RoundRobin returns RUNNING and C is not ticked yet
* | FAILURE | SUCCESS | IDLE | - B returns SUCCESS, so RoundRobin halts all children and
* - returns SUCCESS, next iteration will tick C.
* | RUNNING | IDLE | FAILURE | - C returns FAILURE, so RoundRobin circles and ticks A.
* - A returns RUNNING, so RoundRobin returns RUNNING.
*
* If all children return FAILURE, RoundRobin will return FAILURE
* and halt all children, ending the sequence.
*
* Usage in XML: <RoundRobin>
*/
54 class RoundRobinNode : public BT::ControlNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::RoundRobinNode
* @param name Name for the XML tag for this node
*/
61 explicit RoundRobinNode( const std::string & name );
/**
* @brief A constructor for nav2_behavior_tree::RoundRobinNode
* @param name Name for the XML tag for this node
* @param config BT node configuration
*/
68 RoundRobinNode( const std::string & name, const BT::NodeConfiguration & config );
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
BT::NodeStatus tick( ) override;
/**
* @brief The other ( optional ) override required by a BT action to reset node state
*/
void halt( ) override;
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
*/
static BT::PortsList providedPorts( ) {return {};}
private:
unsigned int current_child_idx_{0};
unsigned int num_failed_children_{0};
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONTROL__ROUND_ROBIN_NODE_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__DISTANCE_CONTROLLER_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__DISTANCE_CONTROLLER_HPP_
#include <memory>
#include <string>
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "tf2_ros/buffer.h"
#include "behaviortree_cpp_v3/decorator_node.h"
namespace nav2_behavior_tree
{
/**
* @brief A BT::DecoratorNode that ticks its child every time the robot
* travels a specified distance
*/
34 class DistanceController : public BT::DecoratorNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::DistanceController
* @param name Name for the XML tag for this node
* @param conf BT node configuration
*/
42 DistanceController(
43 const std::string & name,
44 const BT::NodeConfiguration & conf );
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing node-specific ports
*/
50 static BT::PortsList providedPorts( )
{
return {
BT::InputPort<double>( "distance", 1.0, "Distance" ),
BT::InputPort<std::string>( "global_frame", std::string( "map" ), "Global frame" ),
BT::InputPort<std::string>( "robot_base_frame", std::string( "base_link" ), "Robot base frame" )
};
}
private:
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
BT::NodeStatus tick( ) override;
rclcpp::Node::SharedPtr node_;
std::shared_ptr<tf2_ros::Buffer> tf_;
double transform_tolerance_;
geometry_msgs::msg::PoseStamped start_pose_;
double distance_;
std::string global_frame_;
std::string robot_base_frame_;
bool first_time_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__DISTANCE_CONTROLLER_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATED_CONTROLLER_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATED_CONTROLLER_HPP_
#include <chrono>
#include <string>
#include <vector>
#include "behaviortree_cpp_v3/decorator_node.h"
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
namespace nav2_behavior_tree
{
/**
* @brief A BT::DecoratorNode that ticks its child if the goal was updated
*/
33 class GoalUpdatedController : public BT::DecoratorNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::GoalUpdatedController
* @param name Name for the XML tag for this node
* @param conf BT node configuration
*/
41 GoalUpdatedController(
42 const std::string & name,
43 const BT::NodeConfiguration & conf );
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing node-specific ports
*/
49 static BT::PortsList providedPorts( )
{
return {};
}
private:
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
BT::NodeStatus tick( ) override;
bool goal_was_updated_;
geometry_msgs::msg::PoseStamped goal_;
std::vector<geometry_msgs::msg::PoseStamped> goals_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATED_CONTROLLER_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Francisco Martin Rico
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATER_NODE_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATER_NODE_HPP_
#include <memory>
#include <string>
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "behaviortree_cpp_v3/decorator_node.h"
#include "rclcpp/rclcpp.hpp"
namespace nav2_behavior_tree
{
/**
* @brief A BT::DecoratorNode that subscribes to a goal topic and updates
* the current goal on the blackboard
*/
35 class GoalUpdater : public BT::DecoratorNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::GoalUpdater
* @param xml_tag_name Name for the XML tag for this node
* @param conf BT node configuration
*/
43 GoalUpdater(
44 const std::string & xml_tag_name,
45 const BT::NodeConfiguration & conf );
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing node-specific ports
*/
51 static BT::PortsList providedPorts( )
{
return {
BT::InputPort<geometry_msgs::msg::PoseStamped>( "input_goal", "Original Goal" ),
BT::OutputPort<geometry_msgs::msg::PoseStamped>(
"output_goal",
"Received Goal by subscription" ),
};
}
private:
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
BT::NodeStatus tick( ) override;
/**
* @brief Callback function for goal update topic
* @param msg Shared pointer to geometry_msgs::msg::PoseStamped message
*/
void callback_updated_goal( const geometry_msgs::msg::PoseStamped::SharedPtr msg );
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
geometry_msgs::msg::PoseStamped last_goal_received_;
rclcpp::Node::SharedPtr node_;
rclcpp::CallbackGroup::SharedPtr callback_group_;
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__GOAL_UPDATER_NODE_HPP_
1 // Copyright ( c ) 2022 Neobotix GmbH
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__PATH_LONGER_ON_APPROACH_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__PATH_LONGER_ON_APPROACH_HPP_
#include <string>
#include <memory>
#include <limits>
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav_msgs/msg/path.hpp"
#include "behaviortree_cpp_v3/decorator_node.h"
#include "rclcpp/rclcpp.hpp"
namespace nav2_behavior_tree
{
/**
* @brief A BT::DecoratorNode that ticks its child everytime when the length of
* the new path is smaller than the old one by the length given by the user.
*/
34 class PathLongerOnApproach : public BT::DecoratorNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::PathLongerOnApproach
* @param name Name for the XML tag for this node
* @param conf BT node configuration
*/
42 PathLongerOnApproach(
43 const std::string & name,
44 const BT::NodeConfiguration & conf );
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing node-specific ports
*/
50 static BT::PortsList providedPorts( )
{
return {
BT::InputPort<nav_msgs::msg::Path>( "path", "Planned Path" ),
BT::InputPort<double>(
"prox_len", 3.0,
"Proximity length ( m ) for the path to be longer on approach" ),
BT::InputPort<double>(
"length_factor", 2.0,
"Length multiplication factor to check if the path is significantly longer" ),
};
}
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
BT::NodeStatus tick( ) override;
private:
/**
* @brief Checks if the global path is updated
* @param new_path new path to the goal
* @param old_path current path to the goal
* @return whether the path is updated for the current goal
*/
bool isPathUpdated(
nav_msgs::msg::Path & new_path,
nav_msgs::msg::Path & old_path );
/**
* @brief Checks if the robot is in the goal proximity
* @param old_path current path to the goal
* @param prox_leng proximity length from the goal
* @return whether the robot is in the goal proximity
*/
bool isRobotInGoalProximity(
nav_msgs::msg::Path & old_path,
double & prox_leng );
/**
* @brief Checks if the new path is longer
* @param new_path new path to the goal
* @param old_path current path to the goal
* @param length_factor multipler for path length check
* @return whether the new path is longer
*/
bool isNewPathLonger(
nav_msgs::msg::Path & new_path,
nav_msgs::msg::Path & old_path,
double & length_factor );
private:
nav_msgs::msg::Path new_path_;
nav_msgs::msg::Path old_path_;
double prox_len_ = std::numeric_limits<double>::max( );
double length_factor_ = std::numeric_limits<double>::max( );
rclcpp::Node::SharedPtr node_;
bool first_time_ = true;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__PATH_LONGER_ON_APPROACH_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__RATE_CONTROLLER_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__RATE_CONTROLLER_HPP_
#include <chrono>
#include <string>
#include "behaviortree_cpp_v3/decorator_node.h"
namespace nav2_behavior_tree
{
/**
* @brief A BT::DecoratorNode that ticks its child at a specified rate
*/
29 class RateController : public BT::DecoratorNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::RateController
* @param name Name for the XML tag for this node
* @param conf BT node configuration
*/
37 RateController(
38 const std::string & name,
39 const BT::NodeConfiguration & conf );
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing node-specific ports
*/
45 static BT::PortsList providedPorts( )
{
return {
BT::InputPort<double>( "hz", 10.0, "Rate" )
};
}
private:
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
BT::NodeStatus tick( ) override;
std::chrono::time_point<std::chrono::high_resolution_clock> start_;
double period_;
bool first_time_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__RATE_CONTROLLER_HPP_
1 // Copyright ( c ) 2021 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__SINGLE_TRIGGER_NODE_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__SINGLE_TRIGGER_NODE_HPP_
#include <chrono>
#include <string>
#include "behaviortree_cpp_v3/decorator_node.h"
namespace nav2_behavior_tree
{
/**
* @brief A BT::DecoratorNode that triggers its child only once and returns FAILURE
* for every succeeding tick
*/
30 class SingleTrigger : public BT::DecoratorNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::SingleTrigger
* @param name Name for the XML tag for this node
* @param conf BT node configuration
*/
38 SingleTrigger(
39 const std::string & name,
40 const BT::NodeConfiguration & conf );
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing node-specific ports
*/
46 static BT::PortsList providedPorts( )
{
return {};
}
private:
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
BT::NodeStatus tick( ) override;
bool first_time_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__SINGLE_TRIGGER_NODE_HPP_
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__SPEED_CONTROLLER_HPP_
#define NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__SPEED_CONTROLLER_HPP_
#include <memory>
#include <string>
#include <vector>
#include <deque>
#include "nav_msgs/msg/odometry.hpp"
#include "nav2_util/odometry_utils.hpp"
#include "behaviortree_cpp_v3/decorator_node.h"
namespace nav2_behavior_tree
{
/**
* @brief A BT::DecoratorNode that ticks its child every at a rate proportional to
* the speed of the robot. If the robot travels faster, this node will tick its child at a
* higher frequency and reduce the tick frequency if the robot slows down
*/
37 class SpeedController : public BT::DecoratorNode
{
public:
/**
* @brief A constructor for nav2_behavior_tree::SpeedController
* @param name Name for the XML tag for this node
* @param conf BT node configuration
*/
45 SpeedController(
46 const std::string & name,
47 const BT::NodeConfiguration & conf );
/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing node-specific ports
*/
53 static BT::PortsList providedPorts( )
{
return {
BT::InputPort<double>( "min_rate", 0.1, "Minimum rate" ),
BT::InputPort<double>( "max_rate", 1.0, "Maximum rate" ),
BT::InputPort<double>( "min_speed", 0.0, "Minimum speed" ),
BT::InputPort<double>( "max_speed", 0.5, "Maximum speed" ),
BT::InputPort<double>( "filter_duration", 0.3, "Duration ( secs ) for velocity smoothing filter" )
};
}
private:
/**
* @brief The main override required by a BT action
* @return BT::NodeStatus Status of tick execution
*/
BT::NodeStatus tick( ) override;
/**
* @brief Scale the rate based speed
* @return double Rate scaled by speed limits and clamped
*/
inline double getScaledRate( const double & speed )
{
return std::max(
std::min(
( ( ( speed - min_speed_ ) / d_speed_ ) * d_rate_ ) + min_rate_,
max_rate_ ), min_rate_ );
}
/**
* @brief Update period based on current smoothed speed and reset timer
*/
inline void updatePeriod( )
{
auto velocity = odom_smoother_->getTwist( );
double speed = std::hypot( velocity.linear.x, velocity.linear.y );
double rate = getScaledRate( speed );
91 period_ = 1.0 / rate;
}
rclcpp::Node::SharedPtr node_;
// To keep track of time to reset
rclcpp::Time start_;
// To get a smoothed velocity
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
bool first_tick_;
// Time period after which child node should be ticked
double period_;
// Rates thresholds to tick child node
double min_rate_;
double max_rate_;
double d_rate_;
// Speed thresholds
double min_speed_;
double max_speed_;
double d_speed_;
// current goal
geometry_msgs::msg::PoseStamped goal_;
std::vector<geometry_msgs::msg::PoseStamped> goals_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DECORATOR__SPEED_CONTROLLER_HPP_
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIOR_TREE__ROS_TOPIC_LOGGER_HPP_
#define NAV2_BEHAVIOR_TREE__ROS_TOPIC_LOGGER_HPP_
#include <vector>
#include <memory>
#include <utility>
#include "behaviortree_cpp_v3/loggers/abstract_logger.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_msgs/msg/behavior_tree_log.hpp"
#include "nav2_msgs/msg/behavior_tree_status_change.h"
#include "tf2_ros/buffer_interface.h"
namespace nav2_behavior_tree
{
/**
* @brief A class to publish BT logs on BT status change
*/
34 class RosTopicLogger : public BT::StatusChangeLogger
{
public:
/**
* @brief A constructor for nav2_behavior_tree::RosTopicLogger
* @param ros_node Weak pointer to parent rclcpp::Node
* @param tree BT to monitor
*/
42 RosTopicLogger( const rclcpp::Node::WeakPtr & ros_node, const BT::Tree & tree )
: StatusChangeLogger( tree.rootNode( ) )
{
auto node = ros_node.lock( );
clock_ = node->get_clock( );
logger_ = node->get_logger( );
log_pub_ = node->create_publisher<nav2_msgs::msg::BehaviorTreeLog>(
"behavior_tree_log",
rclcpp::QoS( 10 ) );
}
/**
* @brief Callback function which is called each time BT changes status
* @param timestamp Timestamp of BT status change
* @param node Node that changed status
* @param prev_status Previous status of the node
* @param status Current status of the node
*/
void callback(
BT::Duration timestamp,
const BT::TreeNode & node,
BT::NodeStatus prev_status,
BT::NodeStatus status ) override
{
nav2_msgs::msg::BehaviorTreeStatusChange event;
// BT timestamps are a duration since the epoch. Need to convert to a time_point
// before converting to a msg.
event.timestamp = tf2_ros::toMsg( tf2::TimePoint( timestamp ) );
event.node_name = node.name( );
event.previous_status = toStr( prev_status, false );
event.current_status = toStr( status, false );
event_log_.push_back( std::move( event ) );
RCLCPP_DEBUG(
logger_, "[%.3f]: %25s %s -> %s",
std::chrono::duration<double>( timestamp ).count( ),
node.name( ).c_str( ),
toStr( prev_status, true ).c_str( ),
toStr( status, true ).c_str( ) );
}
/**
* @brief Clear log buffer if any
*/
void flush( ) override
{
if ( !event_log_.empty( ) ) {
auto log_msg = std::make_unique<nav2_msgs::msg::BehaviorTreeLog>( );
91 log_msg->timestamp = clock_->now( );
92 log_msg->event_log = event_log_;
93 log_pub_->publish( std::move( log_msg ) );
94 event_log_.clear( );
}
}
protected:
rclcpp::Clock::SharedPtr clock_;
rclcpp::Logger logger_{rclcpp::get_logger( "bt_navigator" )};
rclcpp::Publisher<nav2_msgs::msg::BehaviorTreeLog>::SharedPtr log_pub_;
std::vector<nav2_msgs::msg::BehaviorTreeStatusChange> event_log_;
};
} // namespace nav2_behavior_tree
#endif // NAV2_BEHAVIOR_TREE__ROS_TOPIC_LOGGER_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <memory>
#include "nav2_behavior_tree/plugins/action/back_up_action.hpp"
namespace nav2_behavior_tree
{
23 BackUpAction::BackUpAction(
24 const std::string & xml_tag_name,
25 const std::string & action_name,
26 const BT::NodeConfiguration & conf )
: BtActionNode<nav2_msgs::action::BackUp>( xml_tag_name, action_name, conf )
{
double dist;
getInput( "backup_dist", dist );
double speed;
getInput( "backup_speed", speed );
double time_allowance;
getInput( "time_allowance", time_allowance );
// Populate the input message
goal_.target.x = dist;
goal_.target.y = 0.0;
goal_.target.z = 0.0;
goal_.speed = speed;
goal_.time_allowance = rclcpp::Duration::from_seconds( time_allowance );
}
44 void BackUpAction::on_tick( )
{
increment_recovery_count( );
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
52 BT_REGISTER_NODES( factory )
{
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::BackUpAction>(
name, "backup", config );
};
factory.registerBuilder<nav2_behavior_tree::BackUpAction>( "BackUp", builder );
}
1 // Copyright ( c ) 2022 Neobotix GmbH
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <memory>
#include "std_msgs/msg/string.hpp"
#include "nav2_behavior_tree/plugins/action/back_up_cancel_node.hpp"
namespace nav2_behavior_tree
{
25 BackUpCancel::BackUpCancel(
26 const std::string & xml_tag_name,
27 const std::string & action_name,
28 const BT::NodeConfiguration & conf )
: BtCancelActionNode<nav2_msgs::action::BackUp>( xml_tag_name, action_name, conf )
{
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
36 BT_REGISTER_NODES( factory )
{
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::BackUpCancel>(
name, "backup", config );
};
factory.registerBuilder<nav2_behavior_tree::BackUpCancel>(
"CancelBackUp", builder );
}
1 // Copyright ( c ) 2019 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <memory>
#include "nav2_behavior_tree/plugins/action/clear_costmap_service.hpp"
namespace nav2_behavior_tree
{
23 ClearEntireCostmapService::ClearEntireCostmapService(
24 const std::string & service_node_name,
25 const BT::NodeConfiguration & conf )
: BtServiceNode<nav2_msgs::srv::ClearEntireCostmap>( service_node_name, conf )
{
}
30 void ClearEntireCostmapService::on_tick( )
{
increment_recovery_count( );
}
35 ClearCostmapExceptRegionService::ClearCostmapExceptRegionService(
36 const std::string & service_node_name,
37 const BT::NodeConfiguration & conf )
: BtServiceNode<nav2_msgs::srv::ClearCostmapExceptRegion>( service_node_name, conf )
{
}
42 void ClearCostmapExceptRegionService::on_tick( )
{
getInput( "reset_distance", request_->reset_distance );
increment_recovery_count( );
}
48 ClearCostmapAroundRobotService::ClearCostmapAroundRobotService(
49 const std::string & service_node_name,
50 const BT::NodeConfiguration & conf )
: BtServiceNode<nav2_msgs::srv::ClearCostmapAroundRobot>( service_node_name, conf )
{
}
55 void ClearCostmapAroundRobotService::on_tick( )
{
getInput( "reset_distance", request_->reset_distance );
increment_recovery_count( );
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
64 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::ClearEntireCostmapService>( "ClearEntireCostmap" );
factory.registerNodeType<nav2_behavior_tree::ClearCostmapExceptRegionService>(
"ClearCostmapExceptRegion" );
factory.registerNodeType<nav2_behavior_tree::ClearCostmapAroundRobotService>(
"ClearCostmapAroundRobot" );
}
1 // Copyright ( c ) 2021 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include <string>
#include <vector>
#include "nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp"
namespace nav2_behavior_tree
{
24 ComputePathThroughPosesAction::ComputePathThroughPosesAction(
25 const std::string & xml_tag_name,
26 const std::string & action_name,
27 const BT::NodeConfiguration & conf )
: BtActionNode<nav2_msgs::action::ComputePathThroughPoses>( xml_tag_name, action_name, conf )
{
}
32 void ComputePathThroughPosesAction::on_tick( )
{
getInput( "goals", goal_.goals );
getInput( "planner_id", goal_.planner_id );
if ( getInput( "start", goal_.start ) ) {
goal_.use_start = true;
}
}
41 BT::NodeStatus ComputePathThroughPosesAction::on_success( )
{
setOutput( "path", result_.result->path );
return BT::NodeStatus::SUCCESS;
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
50 BT_REGISTER_NODES( factory )
{
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::ComputePathThroughPosesAction>(
name, "compute_path_through_poses", config );
};
factory.registerBuilder<nav2_behavior_tree::ComputePathThroughPosesAction>(
"ComputePathThroughPoses", builder );
}
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include <string>
#include "nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp"
namespace nav2_behavior_tree
{
23 ComputePathToPoseAction::ComputePathToPoseAction(
24 const std::string & xml_tag_name,
25 const std::string & action_name,
26 const BT::NodeConfiguration & conf )
: BtActionNode<nav2_msgs::action::ComputePathToPose>( xml_tag_name, action_name, conf )
{
}
31 void ComputePathToPoseAction::on_tick( )
{
getInput( "goal", goal_.goal );
getInput( "planner_id", goal_.planner_id );
if ( getInput( "start", goal_.start ) ) {
goal_.use_start = true;
}
}
40 BT::NodeStatus ComputePathToPoseAction::on_success( )
{
setOutput( "path", result_.result->path );
return BT::NodeStatus::SUCCESS;
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
49 BT_REGISTER_NODES( factory )
{
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::ComputePathToPoseAction>(
name, "compute_path_to_pose", config );
};
factory.registerBuilder<nav2_behavior_tree::ComputePathToPoseAction>(
"ComputePathToPose", builder );
}
1 // Copyright ( c ) 2022 Neobotix GmbH
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <memory>
#include "std_msgs/msg/string.hpp"
#include "nav2_behavior_tree/plugins/action/controller_cancel_node.hpp"
namespace nav2_behavior_tree
{
25 ControllerCancel::ControllerCancel(
26 const std::string & xml_tag_name,
27 const std::string & action_name,
28 const BT::NodeConfiguration & conf )
: BtCancelActionNode<nav2_msgs::action::FollowPath>( xml_tag_name, action_name, conf )
{
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
36 BT_REGISTER_NODES( factory )
{
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::ControllerCancel>(
name, "follow_path", config );
};
factory.registerBuilder<nav2_behavior_tree::ControllerCancel>(
"CancelControl", builder );
}
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Pablo Iñigo Blasco
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <memory>
#include "std_msgs/msg/string.hpp"
#include "nav2_behavior_tree/plugins/action/controller_selector_node.hpp"
#include "rclcpp/rclcpp.hpp"
namespace nav2_behavior_tree
{
using std::placeholders::_1;
30 ControllerSelector::ControllerSelector(
31 const std::string & name,
32 const BT::NodeConfiguration & conf )
: BT::SyncActionNode( name, conf )
{
node_ = config( ).blackboard->get<rclcpp::Node::SharedPtr>( "node" );
callback_group_ = node_->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive,
false );
callback_group_executor_.add_callback_group( callback_group_, node_->get_node_base_interface( ) );
getInput( "topic_name", topic_name_ );
rclcpp::QoS qos( rclcpp::KeepLast( 1 ) );
qos.transient_local( ).reliable( );
rclcpp::SubscriptionOptions sub_option;
sub_option.callback_group = callback_group_;
controller_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
topic_name_,
qos,
std::bind( &ControllerSelector::callbackControllerSelect, this, _1 ),
sub_option );
}
55 BT::NodeStatus ControllerSelector::tick( )
{
callback_group_executor_.spin_some( );
// This behavior always use the last selected controller received from the topic input.
// When no input is specified it uses the default controller.
// If the default controller is not specified then we work in "required controller mode":
// In this mode, the behavior returns failure if the controller selection is not received from
// the topic input.
if ( last_selected_controller_.empty( ) ) {
std::string default_controller;
getInput( "default_controller", default_controller );
if ( default_controller.empty( ) ) {
return BT::NodeStatus::FAILURE;
} else {
last_selected_controller_ = default_controller;
}
}
setOutput( "selected_controller", last_selected_controller_ );
return BT::NodeStatus::SUCCESS;
}
void
80 ControllerSelector::callbackControllerSelect( const std_msgs::msg::String::SharedPtr msg )
{
last_selected_controller_ = msg->data;
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
88 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::ControllerSelector>( "ControllerSelector" );
}
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <memory>
#include "nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp"
namespace nav2_behavior_tree
{
23 DriveOnHeadingAction::DriveOnHeadingAction(
24 const std::string & xml_tag_name,
25 const std::string & action_name,
26 const BT::NodeConfiguration & conf )
: BtActionNode<nav2_msgs::action::DriveOnHeading>( xml_tag_name, action_name, conf )
{
double dist;
getInput( "dist_to_travel", dist );
double speed;
getInput( "speed", speed );
double time_allowance;
getInput( "time_allowance", time_allowance );
// Populate the input message
goal_.target.x = dist;
goal_.target.y = 0.0;
goal_.target.z = 0.0;
goal_.speed = speed;
goal_.time_allowance = rclcpp::Duration::from_seconds( time_allowance );
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
47 BT_REGISTER_NODES( factory )
{
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::DriveOnHeadingAction>(
name, "drive_on_heading", config );
};
factory.registerBuilder<nav2_behavior_tree::DriveOnHeadingAction>( "DriveOnHeading", builder );
}
1 // Copyright ( c ) 2022 Neobotix GmbH
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <memory>
#include "std_msgs/msg/string.hpp"
#include "nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.hpp"
namespace nav2_behavior_tree
{
25 DriveOnHeadingCancel::DriveOnHeadingCancel(
26 const std::string & xml_tag_name,
27 const std::string & action_name,
28 const BT::NodeConfiguration & conf )
: BtCancelActionNode<nav2_msgs::action::DriveOnHeading>( xml_tag_name, action_name, conf )
{
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
36 BT_REGISTER_NODES( factory )
{
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::DriveOnHeadingCancel>(
name, "drive_on_heading", config );
};
factory.registerBuilder<nav2_behavior_tree::DriveOnHeadingCancel>(
"CancelDriveOnHeading", builder );
}
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include <string>
#include "nav2_behavior_tree/plugins/action/follow_path_action.hpp"
namespace nav2_behavior_tree
{
23 FollowPathAction::FollowPathAction(
24 const std::string & xml_tag_name,
25 const std::string & action_name,
26 const BT::NodeConfiguration & conf )
: BtActionNode<nav2_msgs::action::FollowPath>( xml_tag_name, action_name, conf )
{
}
31 void FollowPathAction::on_tick( )
{
getInput( "path", goal_.path );
getInput( "controller_id", goal_.controller_id );
getInput( "goal_checker_id", goal_.goal_checker_id );
}
38 void FollowPathAction::on_wait_for_result(
39 std::shared_ptr<const nav2_msgs::action::FollowPath::Feedback>/*feedback*/ )
{
// Grab the new path
nav_msgs::msg::Path new_path;
getInput( "path", new_path );
// Check if it is not same with the current one
if ( goal_.path != new_path ) {
// the action server on the next loop iteration
goal_.path = new_path;
goal_updated_ = true;
}
std::string new_controller_id;
getInput( "controller_id", new_controller_id );
if ( goal_.controller_id != new_controller_id ) {
goal_.controller_id = new_controller_id;
goal_updated_ = true;
}
std::string new_goal_checker_id;
getInput( "goal_checker_id", new_goal_checker_id );
if ( goal_.goal_checker_id != new_goal_checker_id ) {
goal_.goal_checker_id = new_goal_checker_id;
goal_updated_ = true;
}
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
72 BT_REGISTER_NODES( factory )
{
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::FollowPathAction>(
name, "follow_path", config );
};
factory.registerBuilder<nav2_behavior_tree::FollowPathAction>(
"FollowPath", builder );
}
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Pablo Iñigo Blasco
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <memory>
#include "std_msgs/msg/string.hpp"
#include "nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp"
#include "rclcpp/rclcpp.hpp"
namespace nav2_behavior_tree
{
using std::placeholders::_1;
30 GoalCheckerSelector::GoalCheckerSelector(
31 const std::string & name,
32 const BT::NodeConfiguration & conf )
: BT::SyncActionNode( name, conf )
{
node_ = config( ).blackboard->get<rclcpp::Node::SharedPtr>( "node" );
getInput( "topic_name", topic_name_ );
rclcpp::QoS qos( rclcpp::KeepLast( 1 ) );
qos.transient_local( ).reliable( );
goal_checker_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
topic_name_, qos, std::bind( &GoalCheckerSelector::callbackGoalCheckerSelect, this, _1 ) );
}
46 BT::NodeStatus GoalCheckerSelector::tick( )
{
rclcpp::spin_some( node_ );
// This behavior always use the last selected goal checker received from the topic input.
// When no input is specified it uses the default goal checker.
// If the default goal checker is not specified then we work in "required goal checker mode":
// In this mode, the behavior returns failure if the goal checker selection is not received from
// the topic input.
if ( last_selected_goal_checker_.empty( ) ) {
std::string default_goal_checker;
getInput( "default_goal_checker", default_goal_checker );
if ( default_goal_checker.empty( ) ) {
return BT::NodeStatus::FAILURE;
} else {
last_selected_goal_checker_ = default_goal_checker;
}
}
setOutput( "selected_goal_checker", last_selected_goal_checker_ );
return BT::NodeStatus::SUCCESS;
}
void
71 GoalCheckerSelector::callbackGoalCheckerSelect( const std_msgs::msg::String::SharedPtr msg )
{
last_selected_goal_checker_ = msg->data;
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
79 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::GoalCheckerSelector>( "GoalCheckerSelector" );
}
1 // Copyright ( c ) 2021 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include <string>
#include "nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp"
namespace nav2_behavior_tree
{
23 NavigateThroughPosesAction::NavigateThroughPosesAction(
24 const std::string & xml_tag_name,
25 const std::string & action_name,
26 const BT::NodeConfiguration & conf )
: BtActionNode<nav2_msgs::action::NavigateThroughPoses>( xml_tag_name, action_name, conf )
{
}
31 void NavigateThroughPosesAction::on_tick( )
{
if ( !getInput( "goals", goal_.poses ) ) {
RCLCPP_ERROR(
node_->get_logger( ),
"NavigateThroughPosesAction: goal not provided" );
return;
}
getInput( "behavior_tree", goal_.behavior_tree );
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
45 BT_REGISTER_NODES( factory )
{
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::NavigateThroughPosesAction>(
name, "navigate_through_poses", config );
};
factory.registerBuilder<nav2_behavior_tree::NavigateThroughPosesAction>(
"NavigateThroughPoses", builder );
}
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include <string>
#include "nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp"
namespace nav2_behavior_tree
{
23 NavigateToPoseAction::NavigateToPoseAction(
24 const std::string & xml_tag_name,
25 const std::string & action_name,
26 const BT::NodeConfiguration & conf )
: BtActionNode<nav2_msgs::action::NavigateToPose>( xml_tag_name, action_name, conf )
{
}
31 void NavigateToPoseAction::on_tick( )
{
if ( !getInput( "goal", goal_.pose ) ) {
RCLCPP_ERROR(
node_->get_logger( ),
"NavigateToPoseAction: goal not provided" );
return;
}
getInput( "behavior_tree", goal_.behavior_tree );
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
45 BT_REGISTER_NODES( factory )
{
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::NavigateToPoseAction>(
name, "navigate_to_pose", config );
};
factory.registerBuilder<nav2_behavior_tree::NavigateToPoseAction>(
"NavigateToPose", builder );
}
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Pablo Iñigo Blasco
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <memory>
#include "std_msgs/msg/string.hpp"
#include "nav2_behavior_tree/plugins/action/planner_selector_node.hpp"
#include "rclcpp/rclcpp.hpp"
namespace nav2_behavior_tree
{
using std::placeholders::_1;
30 PlannerSelector::PlannerSelector(
31 const std::string & name,
32 const BT::NodeConfiguration & conf )
: BT::SyncActionNode( name, conf )
{
node_ = config( ).blackboard->get<rclcpp::Node::SharedPtr>( "node" );
callback_group_ = node_->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive,
false );
callback_group_executor_.add_callback_group( callback_group_, node_->get_node_base_interface( ) );
getInput( "topic_name", topic_name_ );
rclcpp::QoS qos( rclcpp::KeepLast( 1 ) );
qos.transient_local( ).reliable( );
rclcpp::SubscriptionOptions sub_option;
sub_option.callback_group = callback_group_;
planner_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
topic_name_,
qos,
std::bind( &PlannerSelector::callbackPlannerSelect, this, _1 ),
sub_option );
}
55 BT::NodeStatus PlannerSelector::tick( )
{
callback_group_executor_.spin_some( );
// This behavior always use the last selected planner received from the topic input.
// When no input is specified it uses the default planner.
// If the default planner is not specified then we work in "required planner mode":
// In this mode, the behavior returns failure if the planner selection is not received from
// the topic input.
if ( last_selected_planner_.empty( ) ) {
std::string default_planner;
getInput( "default_planner", default_planner );
if ( default_planner.empty( ) ) {
return BT::NodeStatus::FAILURE;
} else {
last_selected_planner_ = default_planner;
}
}
setOutput( "selected_planner", last_selected_planner_ );
return BT::NodeStatus::SUCCESS;
}
void
80 PlannerSelector::callbackPlannerSelect( const std_msgs::msg::String::SharedPtr msg )
{
last_selected_planner_ = msg->data;
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
88 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::PlannerSelector>( "PlannerSelector" );
}
1 // Copyright ( c ) 2019 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include "nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp"
namespace nav2_behavior_tree
{
21 ReinitializeGlobalLocalizationService::ReinitializeGlobalLocalizationService(
22 const std::string & service_node_name,
23 const BT::NodeConfiguration & conf )
: BtServiceNode<std_srvs::srv::Empty>( service_node_name, conf )
{}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
30 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::ReinitializeGlobalLocalizationService>(
"ReinitializeGlobalLocalization" );
}
1 // Copyright ( c ) 2021 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <memory>
#include <limits>
#include "nav_msgs/msg/path.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp"
namespace nav2_behavior_tree
{
27 RemovePassedGoals::RemovePassedGoals(
28 const std::string & name,
29 const BT::NodeConfiguration & conf )
: BT::ActionNodeBase( name, conf ),
viapoint_achieved_radius_( 0.5 )
{
getInput( "radius", viapoint_achieved_radius_ );
getInput( "global_frame", global_frame_ );
getInput( "robot_base_frame", robot_base_frame_ );
tf_ = config( ).blackboard->get<std::shared_ptr<tf2_ros::Buffer>>( "tf_buffer" );
auto node = config( ).blackboard->get<rclcpp::Node::SharedPtr>( "node" );
node->get_parameter( "transform_tolerance", transform_tolerance_ );
}
42 inline BT::NodeStatus RemovePassedGoals::tick( )
{
setStatus( BT::NodeStatus::RUNNING );
Goals goal_poses;
getInput( "input_goals", goal_poses );
if ( goal_poses.empty( ) ) {
setOutput( "output_goals", goal_poses );
return BT::NodeStatus::SUCCESS;
}
using namespace nav2_util::geometry_utils; // NOLINT
geometry_msgs::msg::PoseStamped current_pose;
if ( !nav2_util::getCurrentPose(
current_pose, *tf_, global_frame_, robot_base_frame_,
transform_tolerance_ ) )
{
return BT::NodeStatus::FAILURE;
}
double dist_to_goal;
while ( goal_poses.size( ) > 1 ) {
dist_to_goal = euclidean_distance( goal_poses[0].pose, current_pose.pose );
if ( dist_to_goal > viapoint_achieved_radius_ ) {
break;
}
goal_poses.erase( goal_poses.begin( ) );
}
setOutput( "output_goals", goal_poses );
return BT::NodeStatus::SUCCESS;
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
83 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::RemovePassedGoals>( "RemovePassedGoals" );
}
1 // Copyright ( c ) 2021 RoboTech Vision
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include <string>
#include "nav2_behavior_tree/plugins/action/smooth_path_action.hpp"
namespace nav2_behavior_tree
{
24 SmoothPathAction::SmoothPathAction(
25 const std::string & xml_tag_name,
26 const std::string & action_name,
27 const BT::NodeConfiguration & conf )
: BtActionNode<nav2_msgs::action::SmoothPath>( xml_tag_name, action_name, conf )
{
}
32 void SmoothPathAction::on_tick( )
{
getInput( "unsmoothed_path", goal_.path );
getInput( "smoother_id", goal_.smoother_id );
double max_smoothing_duration;
getInput( "max_smoothing_duration", max_smoothing_duration );
goal_.max_smoothing_duration = rclcpp::Duration::from_seconds( max_smoothing_duration );
getInput( "check_for_collisions", goal_.check_for_collisions );
}
42 BT::NodeStatus SmoothPathAction::on_success( )
{
setOutput( "smoothed_path", result_.result->path );
setOutput( "smoothing_duration", rclcpp::Duration( result_.result->smoothing_duration ).seconds( ) );
setOutput( "was_completed", result_.result->was_completed );
return BT::NodeStatus::SUCCESS;
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
53 BT_REGISTER_NODES( factory )
{
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::SmoothPathAction>(
name, "smooth_path", config );
};
factory.registerBuilder<nav2_behavior_tree::SmoothPathAction>(
"SmoothPath", builder );
}
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <memory>
#include "nav2_behavior_tree/plugins/action/spin_action.hpp"
namespace nav2_behavior_tree
{
23 SpinAction::SpinAction(
24 const std::string & xml_tag_name,
25 const std::string & action_name,
26 const BT::NodeConfiguration & conf )
: BtActionNode<nav2_msgs::action::Spin>( xml_tag_name, action_name, conf )
{
double dist;
getInput( "spin_dist", dist );
double time_allowance;
getInput( "time_allowance", time_allowance );
goal_.target_yaw = dist;
goal_.time_allowance = rclcpp::Duration::from_seconds( time_allowance );
getInput( "is_recovery", is_recovery_ );
}
38 void SpinAction::on_tick( )
{
if ( is_recovery_ ) {
increment_recovery_count( );
}
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
48 BT_REGISTER_NODES( factory )
{
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::SpinAction>( name, "spin", config );
};
factory.registerBuilder<nav2_behavior_tree::SpinAction>( "Spin", builder );
}
1 // Copyright ( c ) 2022 Neobotix GmbH
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <memory>
#include "std_msgs/msg/string.hpp"
#include "nav2_behavior_tree/plugins/action/spin_cancel_node.hpp"
namespace nav2_behavior_tree
{
25 SpinCancel::SpinCancel(
26 const std::string & xml_tag_name,
27 const std::string & action_name,
28 const BT::NodeConfiguration & conf )
: BtCancelActionNode<nav2_msgs::action::Spin>( xml_tag_name, action_name, conf )
{
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
36 BT_REGISTER_NODES( factory )
{
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::SpinCancel>(
name, "spin", config );
};
factory.registerBuilder<nav2_behavior_tree::SpinCancel>(
"CancelSpin", builder );
}
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Francisco Martin Rico
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <memory>
#include <limits>
#include "nav_msgs/msg/path.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "behaviortree_cpp_v3/decorator_node.h"
#include "nav2_behavior_tree/plugins/action/truncate_path_action.hpp"
namespace nav2_behavior_tree
{
30 TruncatePath::TruncatePath(
31 const std::string & name,
32 const BT::NodeConfiguration & conf )
: BT::ActionNodeBase( name, conf ),
distance_( 1.0 )
{
getInput( "distance", distance_ );
}
39 inline BT::NodeStatus TruncatePath::tick( )
{
setStatus( BT::NodeStatus::RUNNING );
nav_msgs::msg::Path input_path;
getInput( "input_path", input_path );
if ( input_path.poses.empty( ) ) {
setOutput( "output_path", input_path );
return BT::NodeStatus::SUCCESS;
}
geometry_msgs::msg::PoseStamped final_pose = input_path.poses.back( );
double distance_to_goal = nav2_util::geometry_utils::euclidean_distance(
input_path.poses.back( ), final_pose );
while ( distance_to_goal < distance_ && input_path.poses.size( ) > 2 ) {
input_path.poses.pop_back( );
distance_to_goal = nav2_util::geometry_utils::euclidean_distance(
input_path.poses.back( ), final_pose );
}
double dx = final_pose.pose.position.x - input_path.poses.back( ).pose.position.x;
double dy = final_pose.pose.position.y - input_path.poses.back( ).pose.position.y;
double final_angle = atan2( dy, dx );
if ( std::isnan( final_angle ) || std::isinf( final_angle ) ) {
RCLCPP_WARN(
config( ).blackboard->get<rclcpp::Node::SharedPtr>( "node" )->get_logger( ),
"Final angle is not valid while truncating path. Setting to 0.0" );
final_angle = 0.0;
}
input_path.poses.back( ).pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
final_angle );
setOutput( "output_path", input_path );
return BT::NodeStatus::SUCCESS;
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
86 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::TruncatePath>( "TruncatePath" );
}
1 // Copyright ( c ) 2021 RoboTech Vision
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <limits>
#include <memory>
#include <string>
#include <vector>
#include "behaviortree_cpp_v3/decorator_node.h"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav_msgs/msg/path.hpp"
#include "tf2_ros/create_timer_ros.h"
#include "nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp"
namespace nav2_behavior_tree
{
32 TruncatePathLocal::TruncatePathLocal(
33 const std::string & name,
34 const BT::NodeConfiguration & conf )
: BT::ActionNodeBase( name, conf )
{
tf_buffer_ =
config( ).blackboard->template get<std::shared_ptr<tf2_ros::Buffer>>(
"tf_buffer" );
}
42 inline BT::NodeStatus TruncatePathLocal::tick( )
{
setStatus( BT::NodeStatus::RUNNING );
double distance_forward, distance_backward;
geometry_msgs::msg::PoseStamped pose;
double angular_distance_weight;
double max_robot_pose_search_dist;
getInput( "distance_forward", distance_forward );
getInput( "distance_backward", distance_backward );
getInput( "angular_distance_weight", angular_distance_weight );
getInput( "max_robot_pose_search_dist", max_robot_pose_search_dist );
bool path_pruning = std::isfinite( max_robot_pose_search_dist );
nav_msgs::msg::Path new_path;
getInput( "input_path", new_path );
if ( !path_pruning || new_path != path_ ) {
path_ = new_path;
closest_pose_detection_begin_ = path_.poses.begin( );
}
if ( !getRobotPose( path_.header.frame_id, pose ) ) {
return BT::NodeStatus::FAILURE;
}
if ( path_.poses.empty( ) ) {
setOutput( "output_path", path_ );
return BT::NodeStatus::SUCCESS;
}
auto closest_pose_detection_end = path_.poses.end( );
if ( path_pruning ) {
closest_pose_detection_end = nav2_util::geometry_utils::first_after_integrated_distance(
closest_pose_detection_begin_, path_.poses.end( ), max_robot_pose_search_dist );
}
// find the closest pose on the path
auto current_pose = nav2_util::geometry_utils::min_by(
closest_pose_detection_begin_, closest_pose_detection_end,
[&pose, angular_distance_weight]( const geometry_msgs::msg::PoseStamped & ps ) {
return poseDistance( pose, ps, angular_distance_weight );
} );
if ( path_pruning ) {
closest_pose_detection_begin_ = current_pose;
}
// expand forwards to extract desired length
auto forward_pose_it = nav2_util::geometry_utils::first_after_integrated_distance(
current_pose, path_.poses.end( ), distance_forward );
// expand backwards to extract desired length
// Note: current_pose + 1 is used because reverse iterator points to a cell before it
auto backward_pose_it = nav2_util::geometry_utils::first_after_integrated_distance(
std::reverse_iterator( current_pose + 1 ), path_.poses.rend( ), distance_backward );
nav_msgs::msg::Path output_path;
output_path.header = path_.header;
output_path.poses = std::vector<geometry_msgs::msg::PoseStamped>(
backward_pose_it.base( ), forward_pose_it );
setOutput( "output_path", output_path );
return BT::NodeStatus::SUCCESS;
}
108 inline bool TruncatePathLocal::getRobotPose(
109 std::string path_frame_id, geometry_msgs::msg::PoseStamped & pose )
{
if ( !getInput( "pose", pose ) ) {
std::string robot_frame;
if ( !getInput( "robot_frame", robot_frame ) ) {
RCLCPP_ERROR(
config( ).blackboard->get<rclcpp::Node::SharedPtr>( "node" )->get_logger( ),
"Neither pose nor robot_frame specified for %s", name( ).c_str( ) );
return false;
}
double transform_tolerance;
getInput( "transform_tolerance", transform_tolerance );
if ( !nav2_util::getCurrentPose(
pose, *tf_buffer_, path_frame_id, robot_frame, transform_tolerance ) )
{
RCLCPP_WARN(
config( ).blackboard->get<rclcpp::Node::SharedPtr>( "node" )->get_logger( ),
"Failed to lookup current robot pose for %s", name( ).c_str( ) );
return false;
}
}
return true;
}
double
134 TruncatePathLocal::poseDistance(
135 const geometry_msgs::msg::PoseStamped & pose1,
136 const geometry_msgs::msg::PoseStamped & pose2,
const double angular_distance_weight )
{
double dx = pose1.pose.position.x - pose2.pose.position.x;
double dy = pose1.pose.position.y - pose2.pose.position.y;
// taking angular distance into account in addition to spatial distance
// ( to improve picking a correct pose near cusps and loops )
tf2::Quaternion q1;
tf2::convert( pose1.pose.orientation, q1 );
tf2::Quaternion q2;
tf2::convert( pose2.pose.orientation, q2 );
double da = angular_distance_weight * std::abs( q1.angleShortestPath( q2 ) );
return std::sqrt( dx * dx + dy * dy + da * da );
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
154 BT_REGISTER_NODES( factory ) {
factory.registerNodeType<nav2_behavior_tree::TruncatePathLocal>(
"TruncatePathLocal" );
}
1 // Copyright ( c ) 2018 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <memory>
#include "nav2_behavior_tree/plugins/action/wait_action.hpp"
namespace nav2_behavior_tree
{
23 WaitAction::WaitAction(
24 const std::string & xml_tag_name,
25 const std::string & action_name,
26 const BT::NodeConfiguration & conf )
: BtActionNode<nav2_msgs::action::Wait>( xml_tag_name, action_name, conf )
{
int duration;
getInput( "wait_duration", duration );
if ( duration <= 0 ) {
RCLCPP_WARN(
node_->get_logger( ), "Wait duration is negative or zero "
"( %i ). Setting to positive.", duration );
duration *= -1;
}
goal_.time.sec = duration;
}
41 void WaitAction::on_tick( )
{
increment_recovery_count( );
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
49 BT_REGISTER_NODES( factory )
{
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::WaitAction>( name, "wait", config );
};
factory.registerBuilder<nav2_behavior_tree::WaitAction>( "Wait", builder );
}
1 // Copyright ( c ) 2022 Neobotix GmbH
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <memory>
#include "std_msgs/msg/string.hpp"
#include "nav2_behavior_tree/plugins/action/wait_cancel_node.hpp"
namespace nav2_behavior_tree
{
25 WaitCancel::WaitCancel(
26 const std::string & xml_tag_name,
27 const std::string & action_name,
28 const BT::NodeConfiguration & conf )
: BtCancelActionNode<nav2_msgs::action::Wait>( xml_tag_name, action_name, conf )
{
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
36 BT_REGISTER_NODES( factory )
{
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::WaitCancel>(
name, "wait", config );
};
factory.registerBuilder<nav2_behavior_tree::WaitCancel>(
"CancelWait", builder );
}
1 // Copyright ( c ) 2019 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <memory>
#include "nav2_util/robot_utils.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp"
namespace nav2_behavior_tree
{
27 DistanceTraveledCondition::DistanceTraveledCondition(
28 const std::string & condition_name,
29 const BT::NodeConfiguration & conf )
: BT::ConditionNode( condition_name, conf ),
distance_( 1.0 ),
transform_tolerance_( 0.1 ),
global_frame_( "map" ),
robot_base_frame_( "base_link" )
{
getInput( "distance", distance_ );
getInput( "global_frame", global_frame_ );
getInput( "robot_base_frame", robot_base_frame_ );
node_ = config( ).blackboard->get<rclcpp::Node::SharedPtr>( "node" );
tf_ = config( ).blackboard->get<std::shared_ptr<tf2_ros::Buffer>>( "tf_buffer" );
node_->get_parameter( "transform_tolerance", transform_tolerance_ );
}
44 BT::NodeStatus DistanceTraveledCondition::tick( )
{
if ( status( ) == BT::NodeStatus::IDLE ) {
if ( !nav2_util::getCurrentPose(
start_pose_, *tf_, global_frame_, robot_base_frame_,
transform_tolerance_ ) )
{
RCLCPP_DEBUG( node_->get_logger( ), "Current robot pose is not available." );
}
return BT::NodeStatus::FAILURE;
}
// Determine distance travelled since we've started this iteration
geometry_msgs::msg::PoseStamped current_pose;
if ( !nav2_util::getCurrentPose(
current_pose, *tf_, global_frame_, robot_base_frame_,
transform_tolerance_ ) )
{
RCLCPP_DEBUG( node_->get_logger( ), "Current robot pose is not available." );
return BT::NodeStatus::FAILURE;
}
// Get euclidean distance
auto travelled = nav2_util::geometry_utils::euclidean_distance(
start_pose_.pose, current_pose.pose );
if ( travelled < distance_ ) {
return BT::NodeStatus::FAILURE;
}
// Update start pose
start_pose_ = current_pose;
return BT::NodeStatus::SUCCESS;
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
83 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::DistanceTraveledCondition>( "DistanceTraveled" );
}
1 // Copyright ( c ) 2021 Joshua Wallace
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <vector>
#include <string>
#include "nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp"
namespace nav2_behavior_tree
{
23 GloballyUpdatedGoalCondition::GloballyUpdatedGoalCondition(
24 const std::string & condition_name,
25 const BT::NodeConfiguration & conf )
: BT::ConditionNode( condition_name, conf ),
first_time( true )
{
node_ = config( ).blackboard->get<rclcpp::Node::SharedPtr>( "node" );
}
32 BT::NodeStatus GloballyUpdatedGoalCondition::tick( )
{
if ( first_time ) {
first_time = false;
config( ).blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>( "goals", goals_ );
config( ).blackboard->get<geometry_msgs::msg::PoseStamped>( "goal", goal_ );
return BT::NodeStatus::FAILURE;
}
std::vector<geometry_msgs::msg::PoseStamped> current_goals;
config( ).blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>( "goals", current_goals );
geometry_msgs::msg::PoseStamped current_goal;
config( ).blackboard->get<geometry_msgs::msg::PoseStamped>( "goal", current_goal );
if ( goal_ != current_goal || goals_ != current_goals ) {
goal_ = current_goal;
goals_ = current_goals;
return BT::NodeStatus::SUCCESS;
}
return BT::NodeStatus::FAILURE;
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
58 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::GloballyUpdatedGoalCondition>( "GlobalUpdatedGoal" );
}
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <memory>
#include "nav2_util/robot_utils.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp"
namespace nav2_behavior_tree
{
27 GoalReachedCondition::GoalReachedCondition(
28 const std::string & condition_name,
29 const BT::NodeConfiguration & conf )
: BT::ConditionNode( condition_name, conf ),
initialized_( false ),
global_frame_( "map" ),
robot_base_frame_( "base_link" )
{
getInput( "global_frame", global_frame_ );
getInput( "robot_base_frame", robot_base_frame_ );
}
39 GoalReachedCondition::~GoalReachedCondition( )
{
cleanup( );
}
44 BT::NodeStatus GoalReachedCondition::tick( )
{
if ( !initialized_ ) {
initialize( );
}
if ( isGoalReached( ) ) {
return BT::NodeStatus::SUCCESS;
}
return BT::NodeStatus::FAILURE;
}
56 void GoalReachedCondition::initialize( )
{
node_ = config( ).blackboard->get<rclcpp::Node::SharedPtr>( "node" );
nav2_util::declare_parameter_if_not_declared(
node_, "goal_reached_tol",
rclcpp::ParameterValue( 0.25 ) );
node_->get_parameter_or<double>( "goal_reached_tol", goal_reached_tol_, 0.25 );
tf_ = config( ).blackboard->get<std::shared_ptr<tf2_ros::Buffer>>( "tf_buffer" );
node_->get_parameter( "transform_tolerance", transform_tolerance_ );
initialized_ = true;
}
71 bool GoalReachedCondition::isGoalReached( )
{
geometry_msgs::msg::PoseStamped current_pose;
if ( !nav2_util::getCurrentPose(
current_pose, *tf_, global_frame_, robot_base_frame_, transform_tolerance_ ) )
{
RCLCPP_DEBUG( node_->get_logger( ), "Current robot pose is not available." );
return false;
}
geometry_msgs::msg::PoseStamped goal;
getInput( "goal", goal );
double dx = goal.pose.position.x - current_pose.pose.position.x;
double dy = goal.pose.position.y - current_pose.pose.position.y;
return ( dx * dx + dy * dy ) <= ( goal_reached_tol_ * goal_reached_tol_ );
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
93 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::GoalReachedCondition>( "GoalReached" );
}
1 // Copyright ( c ) 2020 Aitor Miguel Blanco
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <vector>
#include "nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp"
namespace nav2_behavior_tree
{
22 GoalUpdatedCondition::GoalUpdatedCondition(
23 const std::string & condition_name,
24 const BT::NodeConfiguration & conf )
: BT::ConditionNode( condition_name, conf )
{}
28 BT::NodeStatus GoalUpdatedCondition::tick( )
{
if ( status( ) == BT::NodeStatus::IDLE ) {
config( ).blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>( "goals", goals_ );
config( ).blackboard->get<geometry_msgs::msg::PoseStamped>( "goal", goal_ );
return BT::NodeStatus::FAILURE;
}
std::vector<geometry_msgs::msg::PoseStamped> current_goals;
config( ).blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>( "goals", current_goals );
geometry_msgs::msg::PoseStamped current_goal;
config( ).blackboard->get<geometry_msgs::msg::PoseStamped>( "goal", current_goal );
if ( goal_ != current_goal || goals_ != current_goals ) {
goal_ = current_goal;
goals_ = current_goals;
return BT::NodeStatus::SUCCESS;
}
return BT::NodeStatus::FAILURE;
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
53 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::GoalUpdatedCondition>( "GoalUpdated" );
}
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp"
namespace nav2_behavior_tree
{
20 BT::NodeStatus initialPoseReceived( BT::TreeNode & tree_node )
{
auto initPoseReceived = tree_node.config( ).blackboard->get<bool>( "initial_pose_received" );
return initPoseReceived ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
29 BT_REGISTER_NODES( factory )
{
factory.registerSimpleCondition(
"InitialPoseReceived",
std::bind( &nav2_behavior_tree::initialPoseReceived, std::placeholders::_1 ) );
}
1 // Copyright ( c ) 2020 Sarthak Mittal
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include "nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp"
namespace nav2_behavior_tree
{
23 IsBatteryLowCondition::IsBatteryLowCondition(
24 const std::string & condition_name,
25 const BT::NodeConfiguration & conf )
: BT::ConditionNode( condition_name, conf ),
battery_topic_( "/battery_status" ),
min_battery_( 0.0 ),
is_voltage_( false ),
is_battery_low_( false )
{
getInput( "min_battery", min_battery_ );
getInput( "battery_topic", battery_topic_ );
getInput( "is_voltage", is_voltage_ );
node_ = config( ).blackboard->get<rclcpp::Node::SharedPtr>( "node" );
callback_group_ = node_->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive,
false );
callback_group_executor_.add_callback_group( callback_group_, node_->get_node_base_interface( ) );
rclcpp::SubscriptionOptions sub_option;
sub_option.callback_group = callback_group_;
battery_sub_ = node_->create_subscription<sensor_msgs::msg::BatteryState>(
battery_topic_,
rclcpp::SystemDefaultsQoS( ),
std::bind( &IsBatteryLowCondition::batteryCallback, this, std::placeholders::_1 ),
sub_option );
}
50 BT::NodeStatus IsBatteryLowCondition::tick( )
{
callback_group_executor_.spin_some( );
if ( is_battery_low_ ) {
return BT::NodeStatus::SUCCESS;
}
return BT::NodeStatus::FAILURE;
}
59 void IsBatteryLowCondition::batteryCallback( sensor_msgs::msg::BatteryState::SharedPtr msg )
{
if ( is_voltage_ ) {
is_battery_low_ = msg->voltage <= min_battery_;
} else {
is_battery_low_ = msg->percentage <= min_battery_;
}
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
71 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::IsBatteryLowCondition>( "IsBatteryLow" );
}
1 // Copyright ( c ) 2021 Joshua Wallace
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp"
#include <chrono>
#include <memory>
#include <string>
namespace nav2_behavior_tree
{
23 IsPathValidCondition::IsPathValidCondition(
24 const std::string & condition_name,
25 const BT::NodeConfiguration & conf )
: BT::ConditionNode( condition_name, conf )
{
node_ = config( ).blackboard->get<rclcpp::Node::SharedPtr>( "node" );
client_ = node_->create_client<nav2_msgs::srv::IsPathValid>( "is_path_valid" );
server_timeout_ = config( ).blackboard->template get<std::chrono::milliseconds>( "server_timeout" );
getInput<std::chrono::milliseconds>( "server_timeout", server_timeout_ );
}
35 BT::NodeStatus IsPathValidCondition::tick( )
{
nav_msgs::msg::Path path;
getInput( "path", path );
auto request = std::make_shared<nav2_msgs::srv::IsPathValid::Request>( );
request->path = path;
auto result = client_->async_send_request( request );
if ( rclcpp::spin_until_future_complete( node_, result, server_timeout_ ) ==
rclcpp::FutureReturnCode::SUCCESS )
{
if ( result.get( )->is_valid ) {
return BT::NodeStatus::SUCCESS;
}
}
return BT::NodeStatus::FAILURE;
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
58 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::IsPathValidCondition>( "IsPathValid" );
}
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <chrono>
#include "nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp"
using namespace std::chrono_literals; // NOLINT
namespace nav2_behavior_tree
{
25 IsStuckCondition::IsStuckCondition(
26 const std::string & condition_name,
27 const BT::NodeConfiguration & conf )
: BT::ConditionNode( condition_name, conf ),
is_stuck_( false ),
odom_history_size_( 10 ),
current_accel_( 0.0 ),
brake_accel_limit_( -10.0 )
{
node_ = config( ).blackboard->get<rclcpp::Node::SharedPtr>( "node" );
callback_group_ = node_->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive,
false );
callback_group_executor_.add_callback_group( callback_group_, node_->get_node_base_interface( ) );
callback_group_executor_thread = std::thread( [this]( ) {callback_group_executor_.spin( );} );
rclcpp::SubscriptionOptions sub_option;
sub_option.callback_group = callback_group_;
odom_sub_ = node_->create_subscription<nav_msgs::msg::Odometry>(
"odom",
rclcpp::SystemDefaultsQoS( ),
std::bind( &IsStuckCondition::onOdomReceived, this, std::placeholders::_1 ),
sub_option );
RCLCPP_DEBUG( node_->get_logger( ), "Initialized an IsStuckCondition BT node" );
RCLCPP_INFO_ONCE( node_->get_logger( ), "Waiting on odometry" );
}
54 IsStuckCondition::~IsStuckCondition( )
{
RCLCPP_DEBUG( node_->get_logger( ), "Shutting down IsStuckCondition BT node" );
callback_group_executor_.cancel( );
callback_group_executor_thread.join( );
}
61 void IsStuckCondition::onOdomReceived( const typename nav_msgs::msg::Odometry::SharedPtr msg )
{
RCLCPP_INFO_ONCE( node_->get_logger( ), "Got odometry" );
while ( odom_history_.size( ) >= odom_history_size_ ) {
odom_history_.pop_front( );
}
odom_history_.push_back( *msg );
// TODO( orduno ) #383 Move the state calculation and is stuck to robot class
updateStates( );
}
75 BT::NodeStatus IsStuckCondition::tick( )
{
// TODO( orduno ) #383 Once check for is stuck and state calculations are moved to robot class
// this becomes
// if ( robot_state_.isStuck( ) ) {
if ( is_stuck_ ) {
logStuck( "Robot got stuck!" );
return BT::NodeStatus::SUCCESS; // Successfully detected a stuck condition
}
logStuck( "Robot is free" );
return BT::NodeStatus::FAILURE; // Failed to detected a stuck condition
}
90 void IsStuckCondition::logStuck( const std::string & msg ) const
{
static std::string prev_msg;
if ( msg == prev_msg ) {
return;
}
RCLCPP_INFO( node_->get_logger( ), msg.c_str( ) );
prev_msg = msg;
}
102 void IsStuckCondition::updateStates( )
{
// Approximate acceleration
// TODO( orduno ) #400 Smooth out velocity history for better accel approx.
if ( odom_history_.size( ) > 2 ) {
auto curr_odom = odom_history_.end( )[-1];
double curr_time = static_cast<double>( curr_odom.header.stamp.sec );
curr_time += ( static_cast<double>( curr_odom.header.stamp.nanosec ) ) * 1e-9;
auto prev_odom = odom_history_.end( )[-2];
double prev_time = static_cast<double>( prev_odom.header.stamp.sec );
prev_time += ( static_cast<double>( prev_odom.header.stamp.nanosec ) ) * 1e-9;
double dt = curr_time - prev_time;
double vel_diff = static_cast<double>(
curr_odom.twist.twist.linear.x - prev_odom.twist.twist.linear.x );
current_accel_ = vel_diff / dt;
}
is_stuck_ = isStuck( );
}
124 bool IsStuckCondition::isStuck( )
{
// TODO( orduno ) #400 The robot getting stuck can result on different types of motion
// depending on the state prior to getting stuck ( sudden change in accel, not moving at all,
// random oscillations, etc ). For now, we only address the case where there is a sudden
// harsh deceleration. A better approach to capture all situations would be to do a forward
// simulation of the robot motion and compare it with the actual one.
// Detect if robot bumped into something by checking for abnormal deceleration
if ( current_accel_ < brake_accel_limit_ ) {
RCLCPP_DEBUG(
node_->get_logger( ), "Current deceleration is beyond brake limit."
" brake limit: %.2f, current accel: %.2f", brake_accel_limit_, current_accel_ );
return true;
}
return false;
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
147 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::IsStuckCondition>( "IsStuck" );
}
1 // Copyright ( c ) 2022 Joshua Wallace
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <memory>
#include "behaviortree_cpp_v3/condition_node.h"
#include "nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp"
namespace nav2_behavior_tree
{
25 PathExpiringTimerCondition::PathExpiringTimerCondition(
26 const std::string & condition_name,
27 const BT::NodeConfiguration & conf )
: BT::ConditionNode( condition_name, conf ),
period_( 1.0 ),
first_time_( true )
{
getInput( "seconds", period_ );
node_ = config( ).blackboard->get<rclcpp::Node::SharedPtr>( "node" );
}
36 BT::NodeStatus PathExpiringTimerCondition::tick( )
{
if ( first_time_ ) {
getInput( "path", prev_path_ );
first_time_ = false;
start_ = node_->now( );
return BT::NodeStatus::FAILURE;
}
// Grab the new path
nav_msgs::msg::Path path;
getInput( "path", path );
// Reset timer if the path has been updated
if ( prev_path_ != path ) {
prev_path_ = path;
start_ = node_->now( );
}
// Determine how long its been since we've started this iteration
auto elapsed = node_->now( ) - start_;
// Now, get that in seconds
auto seconds = elapsed.seconds( );
if ( seconds < period_ ) {
return BT::NodeStatus::FAILURE;
}
start_ = node_->now( ); // Reset the timer
return BT::NodeStatus::SUCCESS;
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
72 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::PathExpiringTimerCondition>( "PathExpiringTimer" );
}
1 // Copyright ( c ) 2019 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <memory>
#include "behaviortree_cpp_v3/condition_node.h"
#include "nav2_behavior_tree/plugins/condition/time_expired_condition.hpp"
namespace nav2_behavior_tree
{
26 TimeExpiredCondition::TimeExpiredCondition(
27 const std::string & condition_name,
28 const BT::NodeConfiguration & conf )
: BT::ConditionNode( condition_name, conf ),
period_( 1.0 )
{
getInput( "seconds", period_ );
node_ = config( ).blackboard->get<rclcpp::Node::SharedPtr>( "node" );
start_ = node_->now( );
}
37 BT::NodeStatus TimeExpiredCondition::tick( )
{
if ( status( ) == BT::NodeStatus::IDLE ) {
start_ = node_->now( );
return BT::NodeStatus::FAILURE;
}
// Determine how long its been since we've started this iteration
auto elapsed = node_->now( ) - start_;
// Now, get that in seconds
auto seconds = elapsed.seconds( );
if ( seconds < period_ ) {
return BT::NodeStatus::FAILURE;
}
start_ = node_->now( ); // Reset the timer
return BT::NodeStatus::SUCCESS;
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
61 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::TimeExpiredCondition>( "TimeExpired" );
}
1 // Copyright ( c ) 2020 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <chrono>
#include <memory>
#include "nav2_behavior_tree/plugins/condition/transform_available_condition.hpp"
using namespace std::chrono_literals; // NOLINT
namespace nav2_behavior_tree
{
26 TransformAvailableCondition::TransformAvailableCondition(
27 const std::string & condition_name,
28 const BT::NodeConfiguration & conf )
: BT::ConditionNode( condition_name, conf ),
was_found_( false )
{
node_ = config( ).blackboard->get<rclcpp::Node::SharedPtr>( "node" );
tf_ = config( ).blackboard->get<std::shared_ptr<tf2_ros::Buffer>>( "tf_buffer" );
getInput( "child", child_frame_ );
getInput( "parent", parent_frame_ );
if ( child_frame_.empty( ) || parent_frame_.empty( ) ) {
RCLCPP_FATAL(
node_->get_logger( ), "Child frame ( %s ) or parent frame ( %s ) were empty.",
child_frame_.c_str( ), parent_frame_.c_str( ) );
exit( -1 );
}
RCLCPP_DEBUG( node_->get_logger( ), "Initialized an TransformAvailableCondition BT node" );
}
48 TransformAvailableCondition::~TransformAvailableCondition( )
{
RCLCPP_DEBUG( node_->get_logger( ), "Shutting down TransformAvailableCondition BT node" );
}
53 BT::NodeStatus TransformAvailableCondition::tick( )
{
if ( was_found_ ) {
return BT::NodeStatus::SUCCESS;
}
std::string tf_error;
bool found = tf_->canTransform(
child_frame_, parent_frame_, tf2::TimePointZero, &tf_error );
if ( found ) {
was_found_ = true;
return BT::NodeStatus::SUCCESS;
}
RCLCPP_INFO(
node_->get_logger( ), "Transform from %s to %s was not found, tf error: %s",
child_frame_.c_str( ), parent_frame_.c_str( ), tf_error.c_str( ) );
return BT::NodeStatus::FAILURE;
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
78 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::TransformAvailableCondition>( "TransformAvailable" );
}
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <stdexcept>
#include <sstream>
#include <string>
#include "nav2_behavior_tree/plugins/control/pipeline_sequence.hpp"
namespace nav2_behavior_tree
{
24 PipelineSequence::PipelineSequence( const std::string & name )
: BT::ControlNode( name, {} )
{
}
29 PipelineSequence::PipelineSequence(
30 const std::string & name,
31 const BT::NodeConfiguration & config )
: BT::ControlNode( name, config )
{
}
36 BT::NodeStatus PipelineSequence::tick( )
{
for ( std::size_t i = 0; i < children_nodes_.size( ); ++i ) {
auto status = children_nodes_[i]->executeTick( );
switch ( status ) {
case BT::NodeStatus::FAILURE:
ControlNode::haltChildren( );
last_child_ticked_ = 0; // reset
return status;
case BT::NodeStatus::SUCCESS:
// do nothing and continue on to the next child. If it is the last child
// we'll exit the loop and hit the wrap-up code at the end of the method.
break;
case BT::NodeStatus::RUNNING:
if ( i >= last_child_ticked_ ) {
last_child_ticked_ = i;
return status;
}
// else do nothing and continue on to the next child
break;
default:
std::stringstream error_msg;
error_msg << "Invalid node status. Received status " << status <<
"from child " << children_nodes_[i]->name( );
throw std::runtime_error( error_msg.str( ) );
}
}
// Wrap up.
ControlNode::haltChildren( );
last_child_ticked_ = 0; // reset
return BT::NodeStatus::SUCCESS;
}
69 void PipelineSequence::halt( )
{
BT::ControlNode::halt( );
last_child_ticked_ = 0;
}
} // namespace nav2_behavior_tree
77 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::PipelineSequence>( "PipelineSequence" );
}
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include "nav2_behavior_tree/plugins/control/recovery_node.hpp"
namespace nav2_behavior_tree
{
21 RecoveryNode::RecoveryNode(
22 const std::string & name,
23 const BT::NodeConfiguration & conf )
: BT::ControlNode::ControlNode( name, conf ),
current_child_idx_( 0 ),
number_of_retries_( 1 ),
retry_count_( 0 )
{
getInput( "number_of_retries", number_of_retries_ );
}
32 BT::NodeStatus RecoveryNode::tick( )
{
const unsigned children_count = children_nodes_.size( );
if ( children_count != 2 ) {
throw BT::BehaviorTreeException( "Recovery Node '" + name( ) + "' must only have 2 children." );
}
setStatus( BT::NodeStatus::RUNNING );
while ( current_child_idx_ < children_count && retry_count_ <= number_of_retries_ ) {
TreeNode * child_node = children_nodes_[current_child_idx_];
const BT::NodeStatus child_status = child_node->executeTick( );
if ( current_child_idx_ == 0 ) {
switch ( child_status ) {
case BT::NodeStatus::SUCCESS:
{
// reset node and return success when first child returns success
halt( );
return BT::NodeStatus::SUCCESS;
}
case BT::NodeStatus::FAILURE:
{
if ( retry_count_ < number_of_retries_ ) {
// halt first child and tick second child in next iteration
ControlNode::haltChild( 0 );
current_child_idx_++;
break;
} else {
// reset node and return failure when max retries has been exceeded
halt( );
return BT::NodeStatus::FAILURE;
}
}
case BT::NodeStatus::RUNNING:
{
return BT::NodeStatus::RUNNING;
}
default:
{
throw BT::LogicError( "A child node must never return IDLE" );
}
} // end switch
} else if ( current_child_idx_ == 1 ) {
switch ( child_status ) {
case BT::NodeStatus::SUCCESS:
{
// halt second child, increment recovery count, and tick first child in next iteration
ControlNode::haltChild( 1 );
retry_count_++;
current_child_idx_--;
}
break;
case BT::NodeStatus::FAILURE:
{
// reset node and return failure if second child fails
halt( );
return BT::NodeStatus::FAILURE;
}
case BT::NodeStatus::RUNNING:
{
return BT::NodeStatus::RUNNING;
}
default:
{
throw BT::LogicError( "A child node must never return IDLE" );
}
} // end switch
}
} // end while loop
// reset node and return failure
halt( );
return BT::NodeStatus::FAILURE;
}
116 void RecoveryNode::halt( )
{
ControlNode::halt( );
retry_count_ = 0;
current_child_idx_ = 0;
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
126 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::RecoveryNode>( "RecoveryNode" );
}
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include "nav2_behavior_tree/plugins/control/round_robin_node.hpp"
namespace nav2_behavior_tree
{
22 RoundRobinNode::RoundRobinNode( const std::string & name )
: BT::ControlNode::ControlNode( name, {} )
{
}
27 RoundRobinNode::RoundRobinNode(
28 const std::string & name,
29 const BT::NodeConfiguration & config )
: BT::ControlNode( name, config )
{
}
34 BT::NodeStatus RoundRobinNode::tick( )
{
const auto num_children = children_nodes_.size( );
setStatus( BT::NodeStatus::RUNNING );
while ( num_failed_children_ < num_children ) {
TreeNode * child_node = children_nodes_[current_child_idx_];
const BT::NodeStatus child_status = child_node->executeTick( );
switch ( child_status ) {
case BT::NodeStatus::SUCCESS:
{
// Wrap around to the first child
if ( ++current_child_idx_ >= num_children ) {
current_child_idx_ = 0;
}
num_failed_children_ = 0;
ControlNode::haltChildren( );
return BT::NodeStatus::SUCCESS;
}
case BT::NodeStatus::FAILURE:
{
if ( ++current_child_idx_ >= num_children ) {
current_child_idx_ = 0;
}
num_failed_children_++;
break;
}
case BT::NodeStatus::RUNNING:
{
return BT::NodeStatus::RUNNING;
}
default:
{
throw BT::LogicError( "Invalid status return from BT node" );
}
}
}
halt( );
return BT::NodeStatus::FAILURE;
}
81 void RoundRobinNode::halt( )
{
ControlNode::halt( );
current_child_idx_ = 0;
num_failed_children_ = 0;
}
} // namespace nav2_behavior_tree
90 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::RoundRobinNode>( "RoundRobin" );
}
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <chrono>
#include <string>
#include <memory>
#include <cmath>
#include "nav2_util/robot_utils.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "tf2_ros/buffer.h"
#include "behaviortree_cpp_v3/decorator_node.h"
#include "nav2_behavior_tree/plugins/decorator/distance_controller.hpp"
namespace nav2_behavior_tree
{
33 DistanceController::DistanceController(
34 const std::string & name,
35 const BT::NodeConfiguration & conf )
: BT::DecoratorNode( name, conf ),
distance_( 1.0 ),
global_frame_( "map" ),
robot_base_frame_( "base_link" ),
first_time_( false )
{
getInput( "distance", distance_ );
getInput( "global_frame", global_frame_ );
getInput( "robot_base_frame", robot_base_frame_ );
node_ = config( ).blackboard->get<rclcpp::Node::SharedPtr>( "node" );
tf_ = config( ).blackboard->get<std::shared_ptr<tf2_ros::Buffer>>( "tf_buffer" );
node_->get_parameter( "transform_tolerance", transform_tolerance_ );
}
51 inline BT::NodeStatus DistanceController::tick( )
{
if ( status( ) == BT::NodeStatus::IDLE ) {
// Reset the starting position since we're starting a new iteration of
// the distance controller ( moving from IDLE to RUNNING )
if ( !nav2_util::getCurrentPose(
start_pose_, *tf_, global_frame_, robot_base_frame_,
transform_tolerance_ ) )
{
RCLCPP_DEBUG( node_->get_logger( ), "Current robot pose is not available." );
return BT::NodeStatus::FAILURE;
}
first_time_ = true;
}
setStatus( BT::NodeStatus::RUNNING );
// Determine distance travelled since we've started this iteration
geometry_msgs::msg::PoseStamped current_pose;
if ( !nav2_util::getCurrentPose(
current_pose, *tf_, global_frame_, robot_base_frame_,
transform_tolerance_ ) )
{
RCLCPP_DEBUG( node_->get_logger( ), "Current robot pose is not available." );
return BT::NodeStatus::FAILURE;
}
// Get euclidean distance
auto travelled = nav2_util::geometry_utils::euclidean_distance(
start_pose_.pose, current_pose.pose );
// The child gets ticked the first time through and every time the threshold
// distance is crossed. In addition, once the child begins to run, it is
// ticked each time 'til completion
if ( first_time_ || ( child_node_->status( ) == BT::NodeStatus::RUNNING ) ||
travelled >= distance_ )
{
first_time_ = false;
const BT::NodeStatus child_state = child_node_->executeTick( );
switch ( child_state ) {
case BT::NodeStatus::RUNNING:
return BT::NodeStatus::RUNNING;
case BT::NodeStatus::SUCCESS:
if ( !nav2_util::getCurrentPose(
start_pose_, *tf_, global_frame_, robot_base_frame_,
transform_tolerance_ ) )
{
RCLCPP_DEBUG( node_->get_logger( ), "Current robot pose is not available." );
return BT::NodeStatus::FAILURE;
}
return BT::NodeStatus::SUCCESS;
case BT::NodeStatus::FAILURE:
default:
return BT::NodeStatus::FAILURE;
}
}
return status( );
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
117 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::DistanceController>( "DistanceController" );
}
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <chrono>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "behaviortree_cpp_v3/decorator_node.h"
#include "nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp"
namespace nav2_behavior_tree
{
27 GoalUpdatedController::GoalUpdatedController(
28 const std::string & name,
29 const BT::NodeConfiguration & conf )
: BT::DecoratorNode( name, conf )
{
}
34 BT::NodeStatus GoalUpdatedController::tick( )
{
if ( status( ) == BT::NodeStatus::IDLE ) {
// Reset since we're starting a new iteration of
// the goal updated controller ( moving from IDLE to RUNNING )
config( ).blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>( "goals", goals_ );
config( ).blackboard->get<geometry_msgs::msg::PoseStamped>( "goal", goal_ );
goal_was_updated_ = true;
}
setStatus( BT::NodeStatus::RUNNING );
std::vector<geometry_msgs::msg::PoseStamped> current_goals;
config( ).blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>( "goals", current_goals );
geometry_msgs::msg::PoseStamped current_goal;
config( ).blackboard->get<geometry_msgs::msg::PoseStamped>( "goal", current_goal );
if ( goal_ != current_goal || goals_ != current_goals ) {
goal_ = current_goal;
goals_ = current_goals;
goal_was_updated_ = true;
}
// The child gets ticked the first time through and any time the goal has
// changed or preempted. In addition, once the child begins to run, it is ticked each time
// 'til completion
if ( ( child_node_->status( ) == BT::NodeStatus::RUNNING ) || goal_was_updated_ ) {
goal_was_updated_ = false;
const BT::NodeStatus child_state = child_node_->executeTick( );
switch ( child_state ) {
case BT::NodeStatus::RUNNING:
return BT::NodeStatus::RUNNING;
case BT::NodeStatus::SUCCESS:
return BT::NodeStatus::SUCCESS;
case BT::NodeStatus::FAILURE:
default:
return BT::NodeStatus::FAILURE;
}
}
return status( );
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
85 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::GoalUpdatedController>( "GoalUpdatedController" );
}
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Francisco Martin Rico
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <memory>
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "behaviortree_cpp_v3/decorator_node.h"
#include "nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp"
#include "rclcpp/rclcpp.hpp"
namespace nav2_behavior_tree
{
using std::placeholders::_1;
31 GoalUpdater::GoalUpdater(
32 const std::string & name,
33 const BT::NodeConfiguration & conf )
: BT::DecoratorNode( name, conf )
{
node_ = config( ).blackboard->get<rclcpp::Node::SharedPtr>( "node" );
callback_group_ = node_->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive,
false );
callback_group_executor_.add_callback_group( callback_group_, node_->get_node_base_interface( ) );
std::string goal_updater_topic;
node_->get_parameter_or<std::string>( "goal_updater_topic", goal_updater_topic, "goal_update" );
rclcpp::SubscriptionOptions sub_option;
sub_option.callback_group = callback_group_;
goal_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
goal_updater_topic,
10,
std::bind( &GoalUpdater::callback_updated_goal, this, _1 ),
sub_option );
}
54 inline BT::NodeStatus GoalUpdater::tick( )
{
geometry_msgs::msg::PoseStamped goal;
getInput( "input_goal", goal );
callback_group_executor_.spin_some( );
if ( rclcpp::Time( last_goal_received_.header.stamp ) > rclcpp::Time( goal.header.stamp ) ) {
goal = last_goal_received_;
}
setOutput( "output_goal", goal );
return child_node_->executeTick( );
}
void
71 GoalUpdater::callback_updated_goal( const geometry_msgs::msg::PoseStamped::SharedPtr msg )
{
last_goal_received_ = *msg;
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
79 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::GoalUpdater>( "GoalUpdater" );
}
1 // Copyright ( c ) 2022 Neobotix GmbH
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <memory>
#include <vector>
#include "nav2_util/geometry_utils.hpp"
#include "nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp"
namespace nav2_behavior_tree
{
25 PathLongerOnApproach::PathLongerOnApproach(
26 const std::string & name,
27 const BT::NodeConfiguration & conf )
: BT::DecoratorNode( name, conf )
{
node_ = config( ).blackboard->get<rclcpp::Node::SharedPtr>( "node" );
}
33 bool PathLongerOnApproach::isPathUpdated(
34 nav_msgs::msg::Path & new_path,
35 nav_msgs::msg::Path & old_path )
{
return new_path != old_path && old_path.poses.size( ) != 0 &&
new_path.poses.size( ) != 0 &&
old_path.poses.back( ) == new_path.poses.back( );
}
42 bool PathLongerOnApproach::isRobotInGoalProximity(
43 nav_msgs::msg::Path & old_path,
double & prox_leng )
{
return nav2_util::geometry_utils::calculate_path_length( old_path, 0 ) < prox_leng;
}
49 bool PathLongerOnApproach::isNewPathLonger(
50 nav_msgs::msg::Path & new_path,
51 nav_msgs::msg::Path & old_path,
double & length_factor )
{
return nav2_util::geometry_utils::calculate_path_length( new_path, 0 ) >
length_factor * nav2_util::geometry_utils::calculate_path_length(
old_path, 0 );
}
59 inline BT::NodeStatus PathLongerOnApproach::tick( )
{
getInput( "path", new_path_ );
getInput( "prox_len", prox_len_ );
getInput( "length_factor", length_factor_ );
if ( status( ) == BT::NodeStatus::IDLE ) {
// Reset the starting point since we're starting a new iteration of
// PathLongerOnApproach ( moving from IDLE to RUNNING )
first_time_ = true;
}
setStatus( BT::NodeStatus::RUNNING );
// Check if the path is updated and valid, compare the old and the new path length,
// given the goal proximity and check if the new path is longer
if ( isPathUpdated( new_path_, old_path_ ) && isRobotInGoalProximity( old_path_, prox_len_ ) &&
isNewPathLonger( new_path_, old_path_, length_factor_ ) && !first_time_ )
{
const BT::NodeStatus child_state = child_node_->executeTick( );
switch ( child_state ) {
case BT::NodeStatus::RUNNING:
return BT::NodeStatus::RUNNING;
case BT::NodeStatus::SUCCESS:
old_path_ = new_path_;
return BT::NodeStatus::SUCCESS;
case BT::NodeStatus::FAILURE:
old_path_ = new_path_;
return BT::NodeStatus::FAILURE;
default:
old_path_ = new_path_;
return BT::NodeStatus::FAILURE;
}
}
old_path_ = new_path_;
first_time_ = false;
return BT::NodeStatus::SUCCESS;
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
101 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::PathLongerOnApproach>( "PathLongerOnApproach" );
}
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <chrono>
#include <string>
#include "nav2_behavior_tree/plugins/decorator/rate_controller.hpp"
namespace nav2_behavior_tree
{
23 RateController::RateController(
24 const std::string & name,
25 const BT::NodeConfiguration & conf )
: BT::DecoratorNode( name, conf ),
first_time_( false )
{
double hz = 1.0;
getInput( "hz", hz );
period_ = 1.0 / hz;
}
34 BT::NodeStatus RateController::tick( )
{
if ( status( ) == BT::NodeStatus::IDLE ) {
// Reset the starting point since we're starting a new iteration of
// the rate controller ( moving from IDLE to RUNNING )
start_ = std::chrono::high_resolution_clock::now( );
first_time_ = true;
}
setStatus( BT::NodeStatus::RUNNING );
// Determine how long its been since we've started this iteration
auto now = std::chrono::high_resolution_clock::now( );
auto elapsed = now - start_;
// Now, get that in seconds
typedef std::chrono::duration<float> float_seconds;
auto seconds = std::chrono::duration_cast<float_seconds>( elapsed );
// The child gets ticked the first time through and any time the period has
// expired. In addition, once the child begins to run, it is ticked each time
// 'til completion
if ( first_time_ || ( child_node_->status( ) == BT::NodeStatus::RUNNING ) ||
seconds.count( ) >= period_ )
{
first_time_ = false;
const BT::NodeStatus child_state = child_node_->executeTick( );
switch ( child_state ) {
case BT::NodeStatus::RUNNING:
return BT::NodeStatus::RUNNING;
case BT::NodeStatus::SUCCESS:
start_ = std::chrono::high_resolution_clock::now( ); // Reset the timer
return BT::NodeStatus::SUCCESS;
case BT::NodeStatus::FAILURE:
default:
return BT::NodeStatus::FAILURE;
}
}
return status( );
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
82 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::RateController>( "RateController" );
}
1 // Copyright ( c ) 2021 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <chrono>
#include <string>
#include "nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp"
namespace nav2_behavior_tree
{
23 SingleTrigger::SingleTrigger(
24 const std::string & name,
25 const BT::NodeConfiguration & conf )
: BT::DecoratorNode( name, conf ),
first_time_( true )
{
}
31 BT::NodeStatus SingleTrigger::tick( )
{
if ( status( ) == BT::NodeStatus::IDLE ) {
first_time_ = true;
}
setStatus( BT::NodeStatus::RUNNING );
if ( first_time_ ) {
const BT::NodeStatus child_state = child_node_->executeTick( );
switch ( child_state ) {
case BT::NodeStatus::RUNNING:
return BT::NodeStatus::RUNNING;
case BT::NodeStatus::SUCCESS:
first_time_ = false;
return BT::NodeStatus::SUCCESS;
case BT::NodeStatus::FAILURE:
first_time_ = false;
return BT::NodeStatus::FAILURE;
default:
first_time_ = false;
return BT::NodeStatus::FAILURE;
}
}
return BT::NodeStatus::FAILURE;
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
66 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::SingleTrigger>( "SingleTrigger" );
}
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <memory>
#include <vector>
#include "nav2_util/geometry_utils.hpp"
#include "nav2_behavior_tree/plugins/decorator/speed_controller.hpp"
namespace nav2_behavior_tree
{
26 SpeedController::SpeedController(
27 const std::string & name,
28 const BT::NodeConfiguration & conf )
: BT::DecoratorNode( name, conf ),
first_tick_( false ),
period_( 1.0 ),
min_rate_( 0.1 ),
max_rate_( 1.0 ),
min_speed_( 0.0 ),
max_speed_( 0.5 )
{
node_ = config( ).blackboard->get<rclcpp::Node::SharedPtr>( "node" );
getInput( "min_rate", min_rate_ );
getInput( "max_rate", max_rate_ );
getInput( "min_speed", min_speed_ );
getInput( "max_speed", max_speed_ );
if ( min_rate_ <= 0.0 || max_rate_ <= 0.0 ) {
std::string err_msg = "SpeedController node cannot have rate <= 0.0";
RCLCPP_FATAL( node_->get_logger( ), err_msg.c_str( ) );
throw BT::BehaviorTreeException( err_msg );
}
d_rate_ = max_rate_ - min_rate_;
d_speed_ = max_speed_ - min_speed_;
double duration = 0.3;
getInput( "filter_duration", duration );
std::string odom_topic;
node_->get_parameter_or( "odom_topic", odom_topic, std::string( "odom" ) );
odom_smoother_ = std::make_shared<nav2_util::OdomSmoother>( node_, duration, odom_topic );
}
60 inline BT::NodeStatus SpeedController::tick( )
{
auto current_goal = config( ).blackboard->get<geometry_msgs::msg::PoseStamped>( "goal" );
auto current_goals =
config( ).blackboard->get<std::vector<geometry_msgs::msg::PoseStamped>>( "goals" );
if ( goal_ != current_goal || goals_ != current_goals ) {
// Reset state and set period to max since we have a new goal
period_ = 1.0 / max_rate_;
start_ = node_->now( );
first_tick_ = true;
goal_ = current_goal;
goals_ = current_goals;
}
setStatus( BT::NodeStatus::RUNNING );
auto elapsed = node_->now( ) - start_;
// The child gets ticked the first time through and any time the period has
// expired. In addition, once the child begins to run, it is ticked each time
// 'til completion
if ( first_tick_ || ( child_node_->status( ) == BT::NodeStatus::RUNNING ) ||
elapsed.seconds( ) >= period_ )
{
first_tick_ = false;
// update period if the last period is exceeded
if ( elapsed.seconds( ) >= period_ ) {
updatePeriod( );
start_ = node_->now( );
}
const BT::NodeStatus child_state = child_node_->executeTick( );
switch ( child_state ) {
case BT::NodeStatus::RUNNING:
return BT::NodeStatus::RUNNING;
case BT::NodeStatus::SUCCESS:
return BT::NodeStatus::SUCCESS;
case BT::NodeStatus::FAILURE:
default:
return BT::NodeStatus::FAILURE;
}
}
return status( );
}
} // namespace nav2_behavior_tree
#include "behaviortree_cpp_v3/bt_factory.h"
114 BT_REGISTER_NODES( factory )
{
factory.registerNodeType<nav2_behavior_tree::SpeedController>( "SpeedController" );
}
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Florian Gramss
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "nav2_behavior_tree/behavior_tree_engine.hpp"
#include <memory>
#include <string>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "behaviortree_cpp_v3/utils/shared_library.h"
namespace nav2_behavior_tree
{
28 BehaviorTreeEngine::BehaviorTreeEngine( const std::vector<std::string> & plugin_libraries )
{
BT::SharedLibrary loader;
for ( const auto & p : plugin_libraries ) {
factory_.registerFromPlugin( loader.getOSName( p ) );
}
}
BtStatus
37 BehaviorTreeEngine::run(
38 BT::Tree * tree,
39 std::function<void( )> onLoop,
40 std::function<bool( )> cancelRequested,
41 std::chrono::milliseconds loopTimeout )
{
rclcpp::WallRate loopRate( loopTimeout );
BT::NodeStatus result = BT::NodeStatus::RUNNING;
// Loop until something happens with ROS or the node completes
try {
while ( rclcpp::ok( ) && result == BT::NodeStatus::RUNNING ) {
if ( cancelRequested( ) ) {
tree->rootNode( )->halt( );
return BtStatus::CANCELED;
}
result = tree->tickRoot( );
onLoop( );
loopRate.sleep( );
}
} catch ( const std::exception & ex ) {
RCLCPP_ERROR(
rclcpp::get_logger( "BehaviorTreeEngine" ),
"Behavior tree threw exception: %s. Exiting with failure.", ex.what( ) );
return BtStatus::FAILED;
}
return ( result == BT::NodeStatus::SUCCESS ) ? BtStatus::SUCCEEDED : BtStatus::FAILED;
}
BT::Tree
71 BehaviorTreeEngine::createTreeFromText(
72 const std::string & xml_string,
73 BT::Blackboard::Ptr blackboard )
{
return factory_.createTreeFromText( xml_string, blackboard );
}
BT::Tree
79 BehaviorTreeEngine::createTreeFromFile(
80 const std::string & file_path,
81 BT::Blackboard::Ptr blackboard )
{
return factory_.createTreeFromFile( file_path, blackboard );
}
// In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state
void
88 BehaviorTreeEngine::haltAllActions( BT::TreeNode * root_node )
{
if ( !root_node ) {
return;
}
// this halt signal should propagate through the entire tree.
root_node->halt( );
// but, just in case...
auto visitor = []( BT::TreeNode * node ) {
if ( node->status( ) == BT::NodeStatus::RUNNING ) {
node->halt( );
}
};
BT::applyRecursiveVisitor( root_node, visitor );
}
} // namespace nav2_behavior_tree
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include "behaviortree_cpp_v3/bt_factory.h"
#include "../../test_action_server.hpp"
#include "nav2_behavior_tree/plugins/action/back_up_action.hpp"
26 class BackUpActionServer : public TestActionServer<nav2_msgs::action::BackUp>
{
public:
29 BackUpActionServer( )
: TestActionServer( "backup" )
{}
protected:
void execute(
const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::BackUp>>
goal_handle )
override
{
nav2_msgs::action::BackUp::Result::SharedPtr result =
std::make_shared<nav2_msgs::action::BackUp::Result>( );
bool return_success = getReturnSuccess( );
if ( return_success ) {
goal_handle->succeed( result );
} else {
goal_handle->abort( result );
}
}
};
class BackUpActionTestFixture : public ::testing::Test
{
public:
static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "backup_action_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds( 20 ) );
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds( 10 ) );
config_->blackboard->set<bool>( "initial_pose_received", false );
config_->blackboard->set<int>( "number_recoveries", 0 );
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::BackUpAction>(
name, "backup", config );
};
factory_->registerBuilder<nav2_behavior_tree::BackUpAction>( "BackUp", builder );
}
static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
action_server_.reset( );
factory_.reset( );
}
void SetUp( ) override
{
config_->blackboard->set( "number_recoveries", 0 );
}
void TearDown( ) override
{
tree_.reset( );
}
static std::shared_ptr<BackUpActionServer> action_server_;
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr BackUpActionTestFixture::node_ = nullptr;
std::shared_ptr<BackUpActionServer> BackUpActionTestFixture::action_server_ = nullptr;
BT::NodeConfiguration * BackUpActionTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory> BackUpActionTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> BackUpActionTestFixture::tree_ = nullptr;
TEST_F( BackUpActionTestFixture, test_ports )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<BackUp />
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
EXPECT_EQ( tree_->rootNode( )->getInput<double>( "backup_dist" ), 0.15 );
EXPECT_EQ( tree_->rootNode( )->getInput<double>( "backup_speed" ), 0.025 );
xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<BackUp backup_dist="2" backup_speed="0.26" />
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
EXPECT_EQ( tree_->rootNode( )->getInput<double>( "backup_dist" ), 2.0 );
EXPECT_EQ( tree_->rootNode( )->getInput<double>( "backup_speed" ), 0.26 );
}
TEST_F( BackUpActionTestFixture, test_tick )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<BackUp backup_dist="2" backup_speed="0.26" />
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
EXPECT_EQ( config_->blackboard->get<int>( "number_recoveries" ), 0 );
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( config_->blackboard->get<int>( "number_recoveries" ), 1 );
auto goal = action_server_->getCurrentGoal( );
EXPECT_EQ( goal->target.x, 2.0 );
EXPECT_EQ( goal->speed, 0.26f );
}
TEST_F( BackUpActionTestFixture, test_failure )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<BackUp backup_dist="2" backup_speed="0.26" />
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
action_server_->setReturnSuccess( false );
EXPECT_EQ( config_->blackboard->get<int>( "number_recoveries" ), 0 );
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS &&
tree_->rootNode( )->status( ) != BT::NodeStatus::FAILURE )
{
tree_->rootNode( )->executeTick( );
}
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::FAILURE );
EXPECT_EQ( config_->blackboard->get<int>( "number_recoveries" ), 1 );
auto goal = action_server_->getCurrentGoal( );
EXPECT_EQ( goal->target.x, 2.0 );
EXPECT_EQ( goal->speed, 0.26f );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
// initialize action server and spin on new thread
BackUpActionTestFixture::action_server_ = std::make_shared<BackUpActionServer>( );
std::thread server_thread( []( ) {
rclcpp::spin( BackUpActionTestFixture::action_server_ );
} );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
server_thread.join( );
return all_successful;
}
// Copyright ( c ) 2022 Neobotix GmbH
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include "behaviortree_cpp_v3/bt_factory.h"
#include "../../test_action_server.hpp"
#include "nav2_behavior_tree/plugins/action/back_up_cancel_node.hpp"
#include "lifecycle_msgs/srv/change_state.hpp"
26 class CancelBackUpServer : public TestActionServer<nav2_msgs::action::BackUp>
{
public:
29 CancelBackUpServer( )
: TestActionServer( "back_up" )
{}
protected:
34 void execute(
const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::BackUp>>
goal_handle )
{
while ( !goal_handle->is_canceling( ) ) {
// BackUping here until goal cancels
std::this_thread::sleep_for( std::chrono::milliseconds( 15 ) );
}
}
};
45 class CancelBackUpActionTestFixture : public ::testing::Test
{
public:
48 static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "cancel_back_up_action_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds( 20 ) );
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds( 10 ) );
client_ = rclcpp_action::create_client<nav2_msgs::action::BackUp>(
node_, "back_up" );
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::BackUpCancel>(
name, "back_up", config );
};
factory_->registerBuilder<nav2_behavior_tree::BackUpCancel>( "CancelBackUp", builder );
}
80 static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
action_server_.reset( );
client_.reset( );
factory_.reset( );
}
void TearDown( ) override
{
tree_.reset( );
}
static std::shared_ptr<CancelBackUpServer> action_server_;
96 static std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::BackUp>> client_;
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr CancelBackUpActionTestFixture::node_ = nullptr;
std::shared_ptr<CancelBackUpServer>
CancelBackUpActionTestFixture::action_server_ = nullptr;
std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::BackUp>>
CancelBackUpActionTestFixture::client_ = nullptr;
BT::NodeConfiguration * CancelBackUpActionTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory>
CancelBackUpActionTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> CancelBackUpActionTestFixture::tree_ = nullptr;
TEST_F( CancelBackUpActionTestFixture, test_ports )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<CancelBackUp name="BackUpCancel"/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
auto send_goal_options = rclcpp_action::Client<nav2_msgs::action::BackUp>::SendGoalOptions( );
// Creating a dummy goal_msg
auto goal_msg = nav2_msgs::action::BackUp::Goal( );
// Setting target pose
goal_msg.target.x = 0.5;
// BackUping for server and sending a goal
client_->wait_for_action_server( );
client_->async_send_goal( goal_msg, send_goal_options );
// Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class
std::this_thread::sleep_for( std::chrono::milliseconds( 15 ) );
// Executing tick
tree_->rootNode( )->executeTick( );
// BT node should return success, once when the goal is cancelled
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
// Adding another test case to check if the goal is infact cancelling
EXPECT_EQ( action_server_->isGoalCancelled( ), true );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
// initialize action server and back_up on new thread
CancelBackUpActionTestFixture::action_server_ = std::make_shared<CancelBackUpServer>( );
std::thread server_thread( []( ) {
rclcpp::spin( CancelBackUpActionTestFixture::action_server_ );
} );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
server_thread.join( );
return all_successful;
}
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <vector>
#include <string>
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "nav2_behavior_tree/bt_action_node.hpp"
#include "test_msgs/action/fibonacci.hpp"
using namespace std::chrono_literals; // NOLINT
using namespace std::placeholders; // NOLINT
34 class FibonacciActionServer : public rclcpp::Node
{
public:
FibonacciActionServer( )
: rclcpp::Node( "fibonacci_node", rclcpp::NodeOptions( ) ),
sleep_duration_( 0ms )
{
this->action_server_ = rclcpp_action::create_server<test_msgs::action::Fibonacci>(
this->get_node_base_interface( ),
this->get_node_clock_interface( ),
this->get_node_logging_interface( ),
this->get_node_waitables_interface( ),
"fibonacci",
std::bind( &FibonacciActionServer::handle_goal, this, _1, _2 ),
std::bind( &FibonacciActionServer::handle_cancel, this, _1 ),
std::bind( &FibonacciActionServer::handle_accepted, this, _1 ) );
}
void setHandleGoalSleepDuration( std::chrono::milliseconds sleep_duration )
{
sleep_duration_ = sleep_duration;
}
protected:
rclcpp_action::GoalResponse handle_goal(
const rclcpp_action::GoalUUID &,
std::shared_ptr<const test_msgs::action::Fibonacci::Goal> )
{
if ( sleep_duration_ > 0ms ) {
std::this_thread::sleep_for( sleep_duration_ );
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse handle_cancel(
const std::shared_ptr<rclcpp_action::ServerGoalHandle<test_msgs::action::Fibonacci>> )
{
return rclcpp_action::CancelResponse::ACCEPT;
}
void handle_accepted(
const std::shared_ptr<rclcpp_action::ServerGoalHandle<test_msgs::action::Fibonacci>> handle )
{
// this needs to return quickly to avoid blocking the executor, so spin up a new thread
if ( handle ) {
const auto goal = handle->get_goal( );
auto result = std::make_shared<test_msgs::action::Fibonacci::Result>( );
if ( goal->order < 0 ) {
handle->abort( result );
return;
}
auto & sequence = result->sequence;
sequence.push_back( 0 );
sequence.push_back( 1 );
for ( int i = 1; ( i < goal->order ) && rclcpp::ok( ); ++i ) {
sequence.push_back( sequence[i] + sequence[i - 1] );
}
handle->succeed( result );
}
}
protected:
rclcpp_action::Server<test_msgs::action::Fibonacci>::SharedPtr action_server_;
std::chrono::milliseconds sleep_duration_;
};
class FibonacciAction : public nav2_behavior_tree::BtActionNode<test_msgs::action::Fibonacci>
{
public:
FibonacciAction(
const std::string & xml_tag_name,
const BT::NodeConfiguration & conf )
: nav2_behavior_tree::BtActionNode<test_msgs::action::Fibonacci>( xml_tag_name, "fibonacci", conf )
{}
void on_tick( ) override
{
getInput( "order", goal_.order );
}
BT::NodeStatus on_success( ) override
{
config( ).blackboard->set<std::vector<int>>( "sequence", result_.result->sequence );
return BT::NodeStatus::SUCCESS;
}
static BT::PortsList providedPorts( )
{
return providedBasicPorts( {BT::InputPort<int>( "order", "Fibonacci order" )} );
}
};
class BTActionNodeTestFixture : public ::testing::Test
{
public:
static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "bt_action_node_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>( "node", node_ );
config_->blackboard->set<std::chrono::milliseconds>( "server_timeout", 20ms );
config_->blackboard->set<std::chrono::milliseconds>( "bt_loop_duration", 10ms );
config_->blackboard->set<bool>( "initial_pose_received", false );
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<FibonacciAction>( name, config );
};
factory_->registerBuilder<FibonacciAction>( "Fibonacci", builder );
}
static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
action_server_.reset( );
factory_.reset( );
}
void SetUp( ) override
{
// initialize action server and spin on new thread
action_server_ = std::make_shared<FibonacciActionServer>( );
server_thread_ = std::make_shared<std::thread>(
[]( ) {
while ( rclcpp::ok( ) && BTActionNodeTestFixture::action_server_ != nullptr ) {
rclcpp::spin_some( BTActionNodeTestFixture::action_server_ );
std::this_thread::sleep_for( 100ns );
}
} );
}
void TearDown( ) override
{
action_server_.reset( );
tree_.reset( );
server_thread_->join( );
server_thread_.reset( );
}
static std::shared_ptr<FibonacciActionServer> action_server_;
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
static std::shared_ptr<std::thread> server_thread_;
};
rclcpp::Node::SharedPtr BTActionNodeTestFixture::node_ = nullptr;
std::shared_ptr<FibonacciActionServer> BTActionNodeTestFixture::action_server_ = nullptr;
BT::NodeConfiguration * BTActionNodeTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory> BTActionNodeTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> BTActionNodeTestFixture::tree_ = nullptr;
std::shared_ptr<std::thread> BTActionNodeTestFixture::server_thread_ = nullptr;
TEST_F( BTActionNodeTestFixture, test_server_timeout_success )
{
// create tree
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Fibonacci order="5" />
</BehaviorTree>
</root> )";
// the server timeout is larger than the goal handling duration
config_->blackboard->set<std::chrono::milliseconds>( "server_timeout", 20ms );
config_->blackboard->set<std::chrono::milliseconds>( "bt_loop_duration", 10ms );
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
// setting a small action server goal handling duration
action_server_->setHandleGoalSleepDuration( 2ms );
// to keep track of the number of ticks it took to reach a terminal result
int ticks = 0;
BT::NodeStatus result = BT::NodeStatus::RUNNING;
// BT loop execution rate
rclcpp::WallRate loopRate( 10ms );
// main BT execution loop
while ( rclcpp::ok( ) && result == BT::NodeStatus::RUNNING ) {
result = tree_->tickRoot( );
ticks++;
loopRate.sleep( );
}
// get calculated fibonacci sequence from blackboard
auto sequence = config_->blackboard->get<std::vector<int>>( "sequence" );
// expected fibonacci sequence for order 5
std::vector<int> expected = {0, 1, 1, 2, 3, 5};
// since the server timeout was larger than the action server goal handling duration
// the BT should have succeeded
EXPECT_EQ( result, BT::NodeStatus::SUCCESS );
// checking the output fibonacci sequence
EXPECT_EQ( sequence.size( ), expected.size( ) );
for ( size_t i = 0; i < expected.size( ); ++i ) {
EXPECT_EQ( sequence[i], expected[i] );
}
// start a new execution cycle with the previous BT to ensure previous state doesn't leak into
// the new cycle
// halt BT for a new execution cycle
tree_->haltTree( );
// setting a large action server goal handling duration
action_server_->setHandleGoalSleepDuration( 100ms );
// reset state variables
ticks = 0;
result = BT::NodeStatus::RUNNING;
// main BT execution loop
while ( rclcpp::ok( ) && result == BT::NodeStatus::RUNNING ) {
result = tree_->tickRoot( );
ticks++;
loopRate.sleep( );
}
// since the server timeout was smaller than the action server goal handling duration
// the BT should have failed
EXPECT_EQ( result, BT::NodeStatus::FAILURE );
// since the server timeout is 20ms and bt loop duration is 10ms, number of ticks should be 2
EXPECT_EQ( ticks, 2 );
}
TEST_F( BTActionNodeTestFixture, test_server_timeout_failure )
{
// create tree
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Fibonacci order="2" />
</BehaviorTree>
</root> )";
// setting a server timeout smaller than the time the action server will take to accept the goal
// to simulate a server timeout scenario
config_->blackboard->set<std::chrono::milliseconds>( "server_timeout", 90ms );
config_->blackboard->set<std::chrono::milliseconds>( "bt_loop_duration", 10ms );
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
// the action server will take 100ms before accepting the goal
action_server_->setHandleGoalSleepDuration( 100ms );
// to keep track of the number of ticks it took to reach a terminal result
int ticks = 0;
BT::NodeStatus result = BT::NodeStatus::RUNNING;
// BT loop execution rate
rclcpp::WallRate loopRate( 10ms );
// main BT execution loop
while ( rclcpp::ok( ) && result == BT::NodeStatus::RUNNING ) {
result = tree_->tickRoot( );
ticks++;
loopRate.sleep( );
}
// since the server timeout was smaller than the action server goal handling duration
// the BT should have failed
EXPECT_EQ( result, BT::NodeStatus::FAILURE );
// since the server timeout is 90ms and bt loop duration is 10ms, number of ticks should be 9
EXPECT_EQ( ticks, 9 );
// start a new execution cycle with the previous BT to ensure previous state doesn't leak into
// the new cycle
// halt BT for a new execution cycle
tree_->haltTree( );
// setting a small action server goal handling duration
action_server_->setHandleGoalSleepDuration( 25ms );
// reset state variables
ticks = 0;
result = BT::NodeStatus::RUNNING;
// main BT execution loop
while ( rclcpp::ok( ) && result == BT::NodeStatus::RUNNING ) {
result = tree_->tickRoot( );
ticks++;
loopRate.sleep( );
}
// since the server timeout was smaller than the action server goal handling duration
// the BT should have failed
EXPECT_EQ( result, BT::NodeStatus::SUCCESS );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include "behaviortree_cpp_v3/bt_factory.h"
#include "../../test_service.hpp"
#include "nav2_behavior_tree/plugins/action/clear_costmap_service.hpp"
26 class ClearEntireCostmapService : public TestService<nav2_msgs::srv::ClearEntireCostmap>
{
public:
29 ClearEntireCostmapService( )
: TestService( "clear_entire_costmap" )
{}
};
34 class ClearEntireCostmapServiceTestFixture : public ::testing::Test
{
public:
37 static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "clear_entire_costmap_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds( 20 ) );
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds( 10 ) );
config_->blackboard->set<bool>( "initial_pose_received", false );
config_->blackboard->set<int>( "number_recoveries", 0 );
factory_->registerNodeType<nav2_behavior_tree::ClearEntireCostmapService>( "ClearEntireCostmap" );
}
62 static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
server_.reset( );
factory_.reset( );
}
void SetUp( ) override
{
config_->blackboard->set( "number_recoveries", 0 );
}
void TearDown( ) override
{
tree_.reset( );
}
static std::shared_ptr<ClearEntireCostmapService> server_;
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr ClearEntireCostmapServiceTestFixture::node_ = nullptr;
std::shared_ptr<ClearEntireCostmapService> ClearEntireCostmapServiceTestFixture::server_ = nullptr;
BT::NodeConfiguration * ClearEntireCostmapServiceTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory> ClearEntireCostmapServiceTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> ClearEntireCostmapServiceTestFixture::tree_ = nullptr;
TEST_F( ClearEntireCostmapServiceTestFixture, test_tick )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<ClearEntireCostmap service_name="clear_entire_costmap"/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
EXPECT_EQ( config_->blackboard->get<int>( "number_recoveries" ), 0 );
EXPECT_EQ( tree_->rootNode( )->executeTick( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( config_->blackboard->get<int>( "number_recoveries" ), 1 );
}
class ClearCostmapExceptRegionService : public TestService<nav2_msgs::srv::ClearCostmapExceptRegion>
{
public:
ClearCostmapExceptRegionService( )
: TestService( "clear_costmap_except_region" )
{}
};
class ClearCostmapExceptRegionServiceTestFixture : public ::testing::Test
{
public:
static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "clear_costmap_except_region_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds( 10 ) );
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds( 10 ) );
config_->blackboard->set<bool>( "initial_pose_received", false );
config_->blackboard->set<int>( "number_recoveries", 0 );
factory_->registerNodeType<nav2_behavior_tree::ClearCostmapExceptRegionService>(
"ClearCostmapExceptRegion" );
}
static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
server_.reset( );
factory_.reset( );
}
void SetUp( ) override
{
config_->blackboard->set( "number_recoveries", 0 );
}
void TearDown( ) override
{
tree_.reset( );
}
static std::shared_ptr<ClearCostmapExceptRegionService> server_;
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr
ClearCostmapExceptRegionServiceTestFixture::node_ = nullptr;
std::shared_ptr<ClearCostmapExceptRegionService>
ClearCostmapExceptRegionServiceTestFixture::server_ = nullptr;
BT::NodeConfiguration
* ClearCostmapExceptRegionServiceTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory>
ClearCostmapExceptRegionServiceTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree>
ClearCostmapExceptRegionServiceTestFixture::tree_ = nullptr;
TEST_F( ClearCostmapExceptRegionServiceTestFixture, test_tick )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<ClearCostmapExceptRegion service_name="clear_costmap_except_region"/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
EXPECT_EQ( config_->blackboard->get<int>( "number_recoveries" ), 0 );
EXPECT_EQ( tree_->rootNode( )->executeTick( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( config_->blackboard->get<int>( "number_recoveries" ), 1 );
}
//******************************************
class ClearCostmapAroundRobotService : public TestService<nav2_msgs::srv::ClearCostmapAroundRobot>
{
public:
ClearCostmapAroundRobotService( )
: TestService( "clear_costmap_around_robot" )
{}
};
class ClearCostmapAroundRobotServiceTestFixture : public ::testing::Test
{
public:
static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "clear_costmap_around_robot_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds( 10 ) );
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds( 10 ) );
config_->blackboard->set<bool>( "initial_pose_received", false );
config_->blackboard->set<int>( "number_recoveries", 0 );
factory_->registerNodeType<nav2_behavior_tree::ClearCostmapAroundRobotService>(
"ClearCostmapAroundRobot" );
}
static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
server_.reset( );
factory_.reset( );
}
void SetUp( ) override
{
config_->blackboard->set( "number_recoveries", 0 );
}
void TearDown( ) override
{
tree_.reset( );
}
static std::shared_ptr<ClearCostmapAroundRobotService> server_;
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr
ClearCostmapAroundRobotServiceTestFixture::node_ = nullptr;
std::shared_ptr<ClearCostmapAroundRobotService>
ClearCostmapAroundRobotServiceTestFixture::server_ = nullptr;
BT::NodeConfiguration
* ClearCostmapAroundRobotServiceTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory>
ClearCostmapAroundRobotServiceTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree>
ClearCostmapAroundRobotServiceTestFixture::tree_ = nullptr;
TEST_F( ClearCostmapAroundRobotServiceTestFixture, test_tick )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<ClearCostmapAroundRobot service_name="clear_costmap_around_robot"/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
EXPECT_EQ( config_->blackboard->get<int>( "number_recoveries" ), 0 );
EXPECT_EQ( tree_->rootNode( )->executeTick( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( config_->blackboard->get<int>( "number_recoveries" ), 1 );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
// initialize service and spin on new thread
ClearEntireCostmapServiceTestFixture::server_ = std::make_shared<ClearEntireCostmapService>( );
std::thread server_thread( []( ) {
rclcpp::spin( ClearEntireCostmapServiceTestFixture::server_ );
} );
ClearCostmapExceptRegionServiceTestFixture::server_ =
std::make_shared<ClearCostmapExceptRegionService>( );
std::thread server_thread_except_region( []( ) {
rclcpp::spin( ClearCostmapExceptRegionServiceTestFixture::server_ );
} );
ClearCostmapAroundRobotServiceTestFixture::server_ =
std::make_shared<ClearCostmapAroundRobotService>( );
std::thread server_thread_around_robot( []( ) {
rclcpp::spin( ClearCostmapAroundRobotServiceTestFixture::server_ );
} );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
server_thread.join( );
server_thread_except_region.join( );
server_thread_around_robot.join( );
return all_successful;
}
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2021 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include <vector>
#include "nav_msgs/msg/path.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "../../test_action_server.hpp"
#include "nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp"
30 class ComputePathThroughPosesActionServer
31 : public TestActionServer<nav2_msgs::action::ComputePathThroughPoses>
{
public:
34 ComputePathThroughPosesActionServer( )
: TestActionServer( "compute_path_through_poses" )
{}
protected:
void execute(
const typename std::shared_ptr<
rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathThroughPoses>> goal_handle )
override
{
const auto goal = goal_handle->get_goal( );
auto result = std::make_shared<nav2_msgs::action::ComputePathThroughPoses::Result>( );
result->path.poses.resize( 2 );
result->path.poses[1].pose.position.x = goal->goals[0].pose.position.x;
if ( goal->use_start ) {
result->path.poses[0].pose.position.x = goal->start.pose.position.x;
} else {
result->path.poses[0].pose.position.x = 0.0;
}
goal_handle->succeed( result );
}
};
class ComputePathThroughPosesActionTestFixture : public ::testing::Test
{
public:
static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "compute_path_through_poses_action_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds( 20 ) );
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds( 10 ) );
config_->blackboard->set<bool>( "initial_pose_received", false );
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::ComputePathThroughPosesAction>(
name, "compute_path_through_poses", config );
};
factory_->registerBuilder<nav2_behavior_tree::ComputePathThroughPosesAction>(
"ComputePathThroughPoses", builder );
}
static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
action_server_.reset( );
factory_.reset( );
}
void TearDown( ) override
{
tree_.reset( );
}
static std::shared_ptr<ComputePathThroughPosesActionServer> action_server_;
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr ComputePathThroughPosesActionTestFixture::node_ = nullptr;
std::shared_ptr<ComputePathThroughPosesActionServer>
ComputePathThroughPosesActionTestFixture::action_server_ = nullptr;
BT::NodeConfiguration * ComputePathThroughPosesActionTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory> ComputePathThroughPosesActionTestFixture::factory_ =
nullptr;
std::shared_ptr<BT::Tree> ComputePathThroughPosesActionTestFixture::tree_ = nullptr;
TEST_F( ComputePathThroughPosesActionTestFixture, test_tick )
{
// create tree
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
// create new goal and set it on blackboard
std::vector<geometry_msgs::msg::PoseStamped> goals;
goals.resize( 1 );
goals[0].pose.position.x = 1.0;
config_->blackboard->set( "goals", goals );
// tick until node succeeds
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
// the goal should have reached our server
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( tree_->rootNode( )->getInput<std::string>( "planner_id" ), std::string( "GridBased" ) );
EXPECT_EQ( action_server_->getCurrentGoal( )->goals[0].pose.position.x, 1.0 );
EXPECT_FALSE( action_server_->getCurrentGoal( )->use_start );
EXPECT_EQ( action_server_->getCurrentGoal( )->planner_id, std::string( "GridBased" ) );
// check if returned path is correct
nav_msgs::msg::Path path;
config_->blackboard->get<nav_msgs::msg::Path>( "path", path );
EXPECT_EQ( path.poses.size( ), 2u );
EXPECT_EQ( path.poses[0].pose.position.x, 0.0 );
EXPECT_EQ( path.poses[1].pose.position.x, 1.0 );
// halt node so another goal can be sent
tree_->rootNode( )->halt( );
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::IDLE );
// set new goal
goals[0].pose.position.x = -2.5;
config_->blackboard->set( "goals", goals );
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( action_server_->getCurrentGoal( )->goals[0].pose.position.x, -2.5 );
config_->blackboard->get<nav_msgs::msg::Path>( "path", path );
EXPECT_EQ( path.poses.size( ), 2u );
EXPECT_EQ( path.poses[0].pose.position.x, 0.0 );
EXPECT_EQ( path.poses[1].pose.position.x, -2.5 );
}
TEST_F( ComputePathThroughPosesActionTestFixture, test_tick_use_start )
{
// create tree
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<ComputePathThroughPoses goals="{goals}" start="{start}" path="{path}" planner_id="GridBased"/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
// create new start and set it on blackboard
geometry_msgs::msg::PoseStamped start;
start.header.stamp = node_->now( );
start.pose.position.x = 2.0;
config_->blackboard->set( "start", start );
// create new goal and set it on blackboard
std::vector<geometry_msgs::msg::PoseStamped> goals;
goals.resize( 1 );
goals[0].pose.position.x = 1.0;
config_->blackboard->set( "goals", goals );
// tick until node succeeds
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
// the goal should have reached our server
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( tree_->rootNode( )->getInput<std::string>( "planner_id" ), std::string( "GridBased" ) );
EXPECT_EQ( action_server_->getCurrentGoal( )->goals[0].pose.position.x, 1.0 );
EXPECT_EQ( action_server_->getCurrentGoal( )->start.pose.position.x, 2.0 );
EXPECT_TRUE( action_server_->getCurrentGoal( )->use_start );
EXPECT_EQ( action_server_->getCurrentGoal( )->planner_id, std::string( "GridBased" ) );
// check if returned path is correct
nav_msgs::msg::Path path;
config_->blackboard->get<nav_msgs::msg::Path>( "path", path );
EXPECT_EQ( path.poses.size( ), 2u );
EXPECT_EQ( path.poses[0].pose.position.x, 2.0 );
EXPECT_EQ( path.poses[1].pose.position.x, 1.0 );
// halt node so another goal can be sent
tree_->rootNode( )->halt( );
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::IDLE );
// set new goal and new start
goals[0].pose.position.x = -2.5;
start.pose.position.x = -1.5;
config_->blackboard->set( "goals", goals );
config_->blackboard->set( "start", start );
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( action_server_->getCurrentGoal( )->goals[0].pose.position.x, -2.5 );
config_->blackboard->get<nav_msgs::msg::Path>( "path", path );
EXPECT_EQ( path.poses.size( ), 2u );
EXPECT_EQ( path.poses[0].pose.position.x, -1.5 );
EXPECT_EQ( path.poses[1].pose.position.x, -2.5 );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
// initialize action server and spin on new thread
ComputePathThroughPosesActionTestFixture::action_server_ =
std::make_shared<ComputePathThroughPosesActionServer>( );
std::thread server_thread( []( ) {
rclcpp::spin( ComputePathThroughPosesActionTestFixture::action_server_ );
} );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
server_thread.join( );
return all_successful;
}
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include "nav_msgs/msg/path.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "../../test_action_server.hpp"
#include "nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp"
29 class ComputePathToPoseActionServer : public TestActionServer<nav2_msgs::action::ComputePathToPose>
{
public:
32 ComputePathToPoseActionServer( )
: TestActionServer( "compute_path_to_pose" )
{}
protected:
void execute(
const typename std::shared_ptr<
rclcpp_action::ServerGoalHandle<nav2_msgs::action::ComputePathToPose>> goal_handle )
override
{
const auto goal = goal_handle->get_goal( );
auto result = std::make_shared<nav2_msgs::action::ComputePathToPose::Result>( );
result->path.poses.resize( 2 );
result->path.poses[1].pose.position.x = goal->goal.pose.position.x;
if ( goal->use_start ) {
result->path.poses[0].pose.position.x = goal->start.pose.position.x;
} else {
result->path.poses[0].pose.position.x = 0.0;
}
goal_handle->succeed( result );
}
};
class ComputePathToPoseActionTestFixture : public ::testing::Test
{
public:
static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "compute_path_to_pose_action_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds( 20 ) );
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds( 10 ) );
config_->blackboard->set<bool>( "initial_pose_received", false );
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::ComputePathToPoseAction>(
name, "compute_path_to_pose", config );
};
factory_->registerBuilder<nav2_behavior_tree::ComputePathToPoseAction>(
"ComputePathToPose", builder );
}
static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
action_server_.reset( );
factory_.reset( );
}
void TearDown( ) override
{
tree_.reset( );
}
static std::shared_ptr<ComputePathToPoseActionServer> action_server_;
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr ComputePathToPoseActionTestFixture::node_ = nullptr;
std::shared_ptr<ComputePathToPoseActionServer>
ComputePathToPoseActionTestFixture::action_server_ = nullptr;
BT::NodeConfiguration * ComputePathToPoseActionTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory> ComputePathToPoseActionTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> ComputePathToPoseActionTestFixture::tree_ = nullptr;
TEST_F( ComputePathToPoseActionTestFixture, test_tick )
{
// create tree
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
// create new goal and set it on blackboard
geometry_msgs::msg::PoseStamped goal;
goal.header.stamp = node_->now( );
goal.pose.position.x = 1.0;
config_->blackboard->set( "goal", goal );
// tick until node succeeds
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
// the goal should have reached our server
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( tree_->rootNode( )->getInput<std::string>( "planner_id" ), std::string( "GridBased" ) );
EXPECT_EQ( action_server_->getCurrentGoal( )->goal.pose.position.x, 1.0 );
EXPECT_FALSE( action_server_->getCurrentGoal( )->use_start );
EXPECT_EQ( action_server_->getCurrentGoal( )->planner_id, std::string( "GridBased" ) );
// check if returned path is correct
nav_msgs::msg::Path path;
config_->blackboard->get<nav_msgs::msg::Path>( "path", path );
EXPECT_EQ( path.poses.size( ), 2u );
EXPECT_EQ( path.poses[0].pose.position.x, 0.0 );
EXPECT_EQ( path.poses[1].pose.position.x, 1.0 );
// halt node so another goal can be sent
tree_->rootNode( )->halt( );
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::IDLE );
// set new goal
goal.pose.position.x = -2.5;
config_->blackboard->set( "goal", goal );
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( action_server_->getCurrentGoal( )->goal.pose.position.x, -2.5 );
config_->blackboard->get<nav_msgs::msg::Path>( "path", path );
EXPECT_EQ( path.poses.size( ), 2u );
EXPECT_EQ( path.poses[0].pose.position.x, 0.0 );
EXPECT_EQ( path.poses[1].pose.position.x, -2.5 );
}
TEST_F( ComputePathToPoseActionTestFixture, test_tick_use_start )
{
// create tree
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<ComputePathToPose goal="{goal}" start="{start}" path="{path}" planner_id="GridBased"/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
// create new start and set it on blackboard
geometry_msgs::msg::PoseStamped start;
start.header.stamp = node_->now( );
start.pose.position.x = 2.0;
config_->blackboard->set( "start", start );
// create new goal and set it on blackboard
geometry_msgs::msg::PoseStamped goal;
goal.header.stamp = node_->now( );
goal.pose.position.x = 1.0;
config_->blackboard->set( "goal", goal );
// tick until node succeeds
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
// the goal should have reached our server
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( tree_->rootNode( )->getInput<std::string>( "planner_id" ), std::string( "GridBased" ) );
EXPECT_EQ( action_server_->getCurrentGoal( )->goal.pose.position.x, 1.0 );
EXPECT_EQ( action_server_->getCurrentGoal( )->start.pose.position.x, 2.0 );
EXPECT_TRUE( action_server_->getCurrentGoal( )->use_start );
EXPECT_EQ( action_server_->getCurrentGoal( )->planner_id, std::string( "GridBased" ) );
// check if returned path is correct
nav_msgs::msg::Path path;
config_->blackboard->get<nav_msgs::msg::Path>( "path", path );
EXPECT_EQ( path.poses.size( ), 2u );
EXPECT_EQ( path.poses[0].pose.position.x, 2.0 );
EXPECT_EQ( path.poses[1].pose.position.x, 1.0 );
// halt node so another goal can be sent
tree_->rootNode( )->halt( );
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::IDLE );
// set new goal and new start
goal.pose.position.x = -2.5;
start.pose.position.x = -1.5;
config_->blackboard->set( "goal", goal );
config_->blackboard->set( "start", start );
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( action_server_->getCurrentGoal( )->goal.pose.position.x, -2.5 );
config_->blackboard->get<nav_msgs::msg::Path>( "path", path );
EXPECT_EQ( path.poses.size( ), 2u );
EXPECT_EQ( path.poses[0].pose.position.x, -1.5 );
EXPECT_EQ( path.poses[1].pose.position.x, -2.5 );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
// initialize action server and spin on new thread
ComputePathToPoseActionTestFixture::action_server_ =
std::make_shared<ComputePathToPoseActionServer>( );
std::thread server_thread( []( ) {
rclcpp::spin( ComputePathToPoseActionTestFixture::action_server_ );
} );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
server_thread.join( );
return all_successful;
}
// Copyright ( c ) 2022 Neobotix GmbH
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include "behaviortree_cpp_v3/bt_factory.h"
#include "../../test_action_server.hpp"
#include "nav2_behavior_tree/plugins/action/controller_cancel_node.hpp"
#include "lifecycle_msgs/srv/change_state.hpp"
26 class CancelControllerServer : public TestActionServer<nav2_msgs::action::FollowPath>
{
public:
29 CancelControllerServer( )
: TestActionServer( "follow_path" )
{}
protected:
34 void execute(
const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath>>
goal_handle )
{
while ( !goal_handle->is_canceling( ) ) {
// waiting here until goal cancels
std::this_thread::sleep_for( std::chrono::milliseconds( 15 ) );
}
}
};
45 class CancelControllerActionTestFixture : public ::testing::Test
{
public:
48 static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "cancel_control_action_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds( 20 ) );
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds( 10 ) );
client_ = rclcpp_action::create_client<nav2_msgs::action::FollowPath>(
node_, "follow_path" );
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::ControllerCancel>(
name, "follow_path", config );
};
factory_->registerBuilder<nav2_behavior_tree::ControllerCancel>( "CancelControl", builder );
}
80 static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
action_server_.reset( );
client_.reset( );
factory_.reset( );
}
void TearDown( ) override
{
tree_.reset( );
}
static std::shared_ptr<CancelControllerServer> action_server_;
96 static std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::FollowPath>> client_;
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr CancelControllerActionTestFixture::node_ = nullptr;
std::shared_ptr<CancelControllerServer>
CancelControllerActionTestFixture::action_server_ = nullptr;
std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::FollowPath>>
CancelControllerActionTestFixture::client_ = nullptr;
BT::NodeConfiguration * CancelControllerActionTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory>
CancelControllerActionTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> CancelControllerActionTestFixture::tree_ = nullptr;
TEST_F( CancelControllerActionTestFixture, test_ports )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<CancelControl name="ControlCancel"/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
auto send_goal_options = rclcpp_action::Client<nav2_msgs::action::FollowPath>::SendGoalOptions( );
// Creating a dummy goal_msg
auto goal_msg = nav2_msgs::action::FollowPath::Goal( );
// Waiting for server and sending a goal
client_->wait_for_action_server( );
client_->async_send_goal( goal_msg, send_goal_options );
// Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class
std::this_thread::sleep_for( std::chrono::milliseconds( 15 ) );
// Executing tick
tree_->rootNode( )->executeTick( );
// BT node should return success, once when the goal is cancelled
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
// Adding another test case to check if the goal is infact cancelling
EXPECT_EQ( action_server_->isGoalCancelled( ), true );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
// initialize action server and spin on new thread
CancelControllerActionTestFixture::action_server_ = std::make_shared<CancelControllerServer>( );
std::thread server_thread( []( ) {
rclcpp::spin( CancelControllerActionTestFixture::action_server_ );
} );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
server_thread.join( );
return all_successful;
}
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Pablo Iñigo Blasco
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include "../../test_action_server.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "nav2_behavior_tree/plugins/action/controller_selector_node.hpp"
#include "nav_msgs/msg/path.hpp"
#include "std_msgs/msg/string.hpp"
28 class ControllerSelectorTestFixture : public ::testing::Test
{
public:
31 static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "controller_selector_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>( "node", node_ );
BT::NodeBuilder builder = []( const std::string & name, const BT::NodeConfiguration & config ) {
return std::make_unique<nav2_behavior_tree::ControllerSelector>( name, config );
};
factory_->registerBuilder<nav2_behavior_tree::ControllerSelector>(
"ControllerSelector",
builder );
}
52 static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
factory_.reset( );
}
void TearDown( ) override
{
tree_.reset( );
}
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr ControllerSelectorTestFixture::node_ = nullptr;
BT::NodeConfiguration * ControllerSelectorTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory> ControllerSelectorTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> ControllerSelectorTestFixture::tree_ = nullptr;
TEST_F( ControllerSelectorTestFixture, test_custom_topic )
{
// create tree
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<ControllerSelector selected_controller="{selected_controller}" default_controller="DWB" topic_name="controller_selector_custom_topic_name"/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
// tick until node succeeds
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
// check default value
std::string selected_controller_result;
config_->blackboard->get( "selected_controller", selected_controller_result );
EXPECT_EQ( selected_controller_result, "DWB" );
std_msgs::msg::String selected_controller_cmd;
selected_controller_cmd.data = "DWC";
rclcpp::QoS qos( rclcpp::KeepLast( 1 ) );
qos.transient_local( ).reliable( );
auto controller_selector_pub =
node_->create_publisher<std_msgs::msg::String>( "controller_selector_custom_topic_name", qos );
// publish a few updates of the selected_controller
auto start = node_->now( );
while ( ( node_->now( ) - start ).seconds( ) < 0.5 ) {
tree_->rootNode( )->executeTick( );
controller_selector_pub->publish( selected_controller_cmd );
rclcpp::spin_some( node_ );
}
// check controller updated
config_->blackboard->get( "selected_controller", selected_controller_result );
EXPECT_EQ( "DWC", selected_controller_result );
}
TEST_F( ControllerSelectorTestFixture, test_default_topic )
{
// create tree
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<ControllerSelector selected_controller="{selected_controller}" default_controller="GridBased"/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
// tick until node succeeds
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
// check default value
std::string selected_controller_result;
config_->blackboard->get( "selected_controller", selected_controller_result );
EXPECT_EQ( selected_controller_result, "GridBased" );
std_msgs::msg::String selected_controller_cmd;
selected_controller_cmd.data = "RRT";
rclcpp::QoS qos( rclcpp::KeepLast( 1 ) );
qos.transient_local( ).reliable( );
auto controller_selector_pub =
node_->create_publisher<std_msgs::msg::String>( "controller_selector", qos );
// publish a few updates of the selected_controller
auto start = node_->now( );
while ( ( node_->now( ) - start ).seconds( ) < 0.5 ) {
tree_->rootNode( )->executeTick( );
controller_selector_pub->publish( selected_controller_cmd );
rclcpp::spin_some( node_ );
}
// check controller updated
config_->blackboard->get( "selected_controller", selected_controller_result );
EXPECT_EQ( "RRT", selected_controller_result );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include "behaviortree_cpp_v3/bt_factory.h"
#include "../../test_action_server.hpp"
#include "nav2_behavior_tree/plugins/action/drive_on_heading_action.hpp"
26 class DriveOnHeadingActionServer : public TestActionServer<nav2_msgs::action::DriveOnHeading>
{
public:
29 DriveOnHeadingActionServer( )
: TestActionServer( "drive_on_heading" )
{}
protected:
void execute(
const typename std::shared_ptr<rclcpp_action::ServerGoalHandle
<nav2_msgs::action::DriveOnHeading>>
goal_handle )
override
{
nav2_msgs::action::DriveOnHeading::Result::SharedPtr result =
std::make_shared<nav2_msgs::action::DriveOnHeading::Result>( );
bool return_success = getReturnSuccess( );
if ( return_success ) {
goal_handle->succeed( result );
} else {
goal_handle->abort( result );
}
}
};
class DriveOnHeadingActionTestFixture : public ::testing::Test
{
public:
static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "drive_on_heading_action_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds( 20 ) );
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds( 10 ) );
config_->blackboard->set<bool>( "initial_pose_received", false );
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::DriveOnHeadingAction>(
name, "drive_on_heading", config );
};
factory_->registerBuilder<nav2_behavior_tree::DriveOnHeadingAction>( "DriveOnHeading", builder );
}
static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
action_server_.reset( );
factory_.reset( );
}
void TearDown( ) override
{
tree_.reset( );
}
static std::shared_ptr<DriveOnHeadingActionServer> action_server_;
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr DriveOnHeadingActionTestFixture::node_ = nullptr;
std::shared_ptr<DriveOnHeadingActionServer>
DriveOnHeadingActionTestFixture::action_server_ = nullptr;
BT::NodeConfiguration * DriveOnHeadingActionTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory> DriveOnHeadingActionTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> DriveOnHeadingActionTestFixture::tree_ = nullptr;
TEST_F( DriveOnHeadingActionTestFixture, test_ports )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<DriveOnHeading />
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
EXPECT_EQ( tree_->rootNode( )->getInput<double>( "dist_to_travel" ), 0.15 );
EXPECT_EQ( tree_->rootNode( )->getInput<double>( "speed" ), 0.025 );
EXPECT_EQ( tree_->rootNode( )->getInput<double>( "time_allowance" ), 10.0 );
xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<DriveOnHeading dist_to_travel="2" speed="0.26" />
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
EXPECT_EQ( tree_->rootNode( )->getInput<double>( "dist_to_travel" ), 2.0 );
EXPECT_EQ( tree_->rootNode( )->getInput<double>( "speed" ), 0.26 );
}
TEST_F( DriveOnHeadingActionTestFixture, test_tick )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<DriveOnHeading dist_to_travel="2" speed="0.26" />
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
auto goal = action_server_->getCurrentGoal( );
EXPECT_EQ( goal->target.x, 2.0 );
EXPECT_EQ( goal->speed, 0.26f );
}
TEST_F( DriveOnHeadingActionTestFixture, test_failure )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<DriveOnHeading dist_to_travel="2" speed="0.26" />
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
action_server_->setReturnSuccess( false );
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS &&
tree_->rootNode( )->status( ) != BT::NodeStatus::FAILURE )
{
tree_->rootNode( )->executeTick( );
}
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::FAILURE );
auto goal = action_server_->getCurrentGoal( );
EXPECT_EQ( goal->target.x, 2.0 );
EXPECT_EQ( goal->speed, 0.26f );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
// initialize action server and spin on new thread
DriveOnHeadingActionTestFixture::action_server_ = std::make_shared<DriveOnHeadingActionServer>( );
std::thread server_thread( []( ) {
rclcpp::spin( DriveOnHeadingActionTestFixture::action_server_ );
} );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
server_thread.join( );
return all_successful;
}
// Copyright ( c ) 2022 Joshua Wallace
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include "behaviortree_cpp_v3/bt_factory.h"
#include "../../test_action_server.hpp"
#include "nav2_behavior_tree/plugins/action/drive_on_heading_cancel_node.hpp"
#include "lifecycle_msgs/srv/change_state.hpp"
26 class CancelDriveOnHeadingServer : public TestActionServer<nav2_msgs::action::DriveOnHeading>
{
public:
29 CancelDriveOnHeadingServer( )
: TestActionServer( "drive_on_heading_cancel" )
{}
protected:
34 void execute(
const typename std::shared_ptr<rclcpp_action::ServerGoalHandle
<nav2_msgs::action::DriveOnHeading>>
goal_handle )
{
while ( !goal_handle->is_canceling( ) ) {
// DriveOnHeadingCancel here until goal cancels
std::this_thread::sleep_for( std::chrono::milliseconds( 15 ) );
}
}
};
46 class CancelDriveOnHeadingTestFixture : public ::testing::Test
{
public:
49 static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "cancel_drive_on_heading_action_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds( 20 ) );
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds( 10 ) );
client_ = rclcpp_action::create_client<nav2_msgs::action::DriveOnHeading>(
node_, "drive_on_heading_cancel" );
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::DriveOnHeadingCancel>(
name, "drive_on_heading_cancel", config );
};
factory_->registerBuilder<nav2_behavior_tree::DriveOnHeadingCancel>(
"CancelDriveOnHeading",
builder );
}
83 static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
action_server_.reset( );
client_.reset( );
factory_.reset( );
}
void TearDown( ) override
{
tree_.reset( );
}
static std::shared_ptr<CancelDriveOnHeadingServer> action_server_;
99 static std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::DriveOnHeading>> client_;
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr CancelDriveOnHeadingTestFixture::node_ = nullptr;
std::shared_ptr<CancelDriveOnHeadingServer>
CancelDriveOnHeadingTestFixture::action_server_ = nullptr;
std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::DriveOnHeading>>
CancelDriveOnHeadingTestFixture::client_ = nullptr;
BT::NodeConfiguration * CancelDriveOnHeadingTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory>
CancelDriveOnHeadingTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> CancelDriveOnHeadingTestFixture::tree_ = nullptr;
TEST_F( CancelDriveOnHeadingTestFixture, test_ports )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<CancelDriveOnHeading name="CancelDriveOnHeading"/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
auto send_goal_options = rclcpp_action::Client
<nav2_msgs::action::DriveOnHeading>::SendGoalOptions( );
// Creating a dummy goal_msg
auto goal_msg = nav2_msgs::action::DriveOnHeading::Goal( );
// Setting target pose
goal_msg.target.x = 0.5;
// DriveOnHeadingCancel for server and sending a goal
client_->wait_for_action_server( );
client_->async_send_goal( goal_msg, send_goal_options );
// Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class
std::this_thread::sleep_for( std::chrono::milliseconds( 15 ) );
// Executing tick
tree_->rootNode( )->executeTick( );
// BT node should return success, once when the goal is cancelled
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
// Adding another test case to check if the goal is infact cancelling
EXPECT_EQ( action_server_->isGoalCancelled( ), true );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
// initialize action server and drive on new thread
CancelDriveOnHeadingTestFixture::action_server_ = std::make_shared<CancelDriveOnHeadingServer>( );
std::thread server_thread( []( ) {
rclcpp::spin( CancelDriveOnHeadingTestFixture::action_server_ );
} );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
server_thread.join( );
return all_successful;
}
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include "nav_msgs/msg/path.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "../../test_action_server.hpp"
#include "nav2_behavior_tree/plugins/action/follow_path_action.hpp"
29 class FollowPathActionServer : public TestActionServer<nav2_msgs::action::FollowPath>
{
public:
32 FollowPathActionServer( )
: TestActionServer( "follow_path" )
{}
protected:
void execute(
const typename std::shared_ptr<
rclcpp_action::ServerGoalHandle<nav2_msgs::action::FollowPath>> goal_handle )
override
{
const auto goal = goal_handle->get_goal( );
auto result = std::make_shared<nav2_msgs::action::FollowPath::Result>( );
goal_handle->succeed( result );
}
};
48 class FollowPathActionTestFixture : public ::testing::Test
{
public:
51 static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "follow_path_action_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds( 20 ) );
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds( 10 ) );
config_->blackboard->set<bool>( "initial_pose_received", false );
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::FollowPathAction>(
name, "follow_path", config );
};
factory_->registerBuilder<nav2_behavior_tree::FollowPathAction>(
"FollowPath", builder );
}
83 static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
action_server_.reset( );
factory_.reset( );
}
void TearDown( ) override
{
tree_.reset( );
}
static std::shared_ptr<FollowPathActionServer> action_server_;
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr FollowPathActionTestFixture::node_ = nullptr;
std::shared_ptr<FollowPathActionServer>
FollowPathActionTestFixture::action_server_ = nullptr;
BT::NodeConfiguration * FollowPathActionTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory> FollowPathActionTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> FollowPathActionTestFixture::tree_ = nullptr;
TEST_F( FollowPathActionTestFixture, test_tick )
{
// create tree
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<FollowPath path="{path}" controller_id="FollowPath"/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
// set new path on blackboard
nav_msgs::msg::Path path;
path.poses.resize( 1 );
path.poses[0].pose.position.x = 1.0;
config_->blackboard->set<nav_msgs::msg::Path>( "path", path );
// tick until node succeeds
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
// the goal should have reached our server
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( tree_->rootNode( )->getInput<std::string>( "controller_id" ), std::string( "FollowPath" ) );
EXPECT_EQ( action_server_->getCurrentGoal( )->path.poses.size( ), 1u );
EXPECT_EQ( action_server_->getCurrentGoal( )->path.poses[0].pose.position.x, 1.0 );
EXPECT_EQ( action_server_->getCurrentGoal( )->controller_id, std::string( "FollowPath" ) );
// halt node so another goal can be sent
tree_->rootNode( )->halt( );
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::IDLE );
// set new goal
path.poses[0].pose.position.x = -2.5;
config_->blackboard->set<nav_msgs::msg::Path>( "path", path );
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( action_server_->getCurrentGoal( )->path.poses.size( ), 1u );
EXPECT_EQ( action_server_->getCurrentGoal( )->path.poses[0].pose.position.x, -2.5 );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
// initialize action server and spin on new thread
FollowPathActionTestFixture::action_server_ =
std::make_shared<FollowPathActionServer>( );
std::thread server_thread( []( ) {
rclcpp::spin( FollowPathActionTestFixture::action_server_ );
} );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
server_thread.join( );
return all_successful;
}
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Pablo Iñigo Blasco
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include "../../test_action_server.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "nav2_behavior_tree/plugins/action/goal_checker_selector_node.hpp"
#include "nav_msgs/msg/path.hpp"
#include "std_msgs/msg/string.hpp"
28 class GoalCheckerSelectorTestFixture : public ::testing::Test
{
public:
31 static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "goal_checker_selector_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>( "node", node_ );
BT::NodeBuilder builder = []( const std::string & name, const BT::NodeConfiguration & config ) {
return std::make_unique<nav2_behavior_tree::GoalCheckerSelector>( name, config );
};
factory_->registerBuilder<nav2_behavior_tree::GoalCheckerSelector>(
"GoalCheckerSelector",
builder );
}
52 static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
factory_.reset( );
}
void TearDown( ) override
{
tree_.reset( );
}
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr GoalCheckerSelectorTestFixture::node_ = nullptr;
BT::NodeConfiguration * GoalCheckerSelectorTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory> GoalCheckerSelectorTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> GoalCheckerSelectorTestFixture::tree_ = nullptr;
TEST_F( GoalCheckerSelectorTestFixture, test_custom_topic )
{
// create tree
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="SimpleGoalCheck" topic_name="goal_checker_selector_custom_topic_name"/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
// tick until node succeeds
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
// check default value
std::string selected_goal_checker_result;
config_->blackboard->get( "selected_goal_checker", selected_goal_checker_result );
EXPECT_EQ( selected_goal_checker_result, "SimpleGoalCheck" );
std_msgs::msg::String selected_goal_checker_cmd;
selected_goal_checker_cmd.data = "AngularGoalChecker";
rclcpp::QoS qos( rclcpp::KeepLast( 1 ) );
qos.transient_local( ).reliable( );
auto goal_checker_selector_pub =
node_->create_publisher<std_msgs::msg::String>( "goal_checker_selector_custom_topic_name", qos );
// publish a few updates of the selected_goal_checker
auto start = node_->now( );
while ( ( node_->now( ) - start ).seconds( ) < 0.5 ) {
tree_->rootNode( )->executeTick( );
goal_checker_selector_pub->publish( selected_goal_checker_cmd );
rclcpp::spin_some( node_ );
}
// check goal_checker updated
config_->blackboard->get( "selected_goal_checker", selected_goal_checker_result );
EXPECT_EQ( "AngularGoalChecker", selected_goal_checker_result );
}
TEST_F( GoalCheckerSelectorTestFixture, test_default_topic )
{
// create tree
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="GridBased"/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
// tick until node succeeds
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
// check default value
std::string selected_goal_checker_result;
config_->blackboard->get( "selected_goal_checker", selected_goal_checker_result );
EXPECT_EQ( selected_goal_checker_result, "GridBased" );
std_msgs::msg::String selected_goal_checker_cmd;
selected_goal_checker_cmd.data = "RRT";
rclcpp::QoS qos( rclcpp::KeepLast( 1 ) );
qos.transient_local( ).reliable( );
auto goal_checker_selector_pub =
node_->create_publisher<std_msgs::msg::String>( "goal_checker_selector", qos );
// publish a few updates of the selected_goal_checker
auto start = node_->now( );
while ( ( node_->now( ) - start ).seconds( ) < 0.5 ) {
tree_->rootNode( )->executeTick( );
goal_checker_selector_pub->publish( selected_goal_checker_cmd );
rclcpp::spin_some( node_ );
}
// check goal_checker updated
config_->blackboard->get( "selected_goal_checker", selected_goal_checker_result );
EXPECT_EQ( "RRT", selected_goal_checker_result );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2021 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include <vector>
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "../../test_action_server.hpp"
#include "nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp"
31 class NavigateThroughPosesActionServer
32 : public TestActionServer<nav2_msgs::action::NavigateThroughPoses>
{
public:
35 NavigateThroughPosesActionServer( )
: TestActionServer( "navigate_through_poses" )
{}
protected:
void execute(
const typename std::shared_ptr<
rclcpp_action::ServerGoalHandle<nav2_msgs::action::NavigateThroughPoses>> goal_handle )
override
{
const auto goal = goal_handle->get_goal( );
auto result = std::make_shared<nav2_msgs::action::NavigateThroughPoses::Result>( );
goal_handle->succeed( result );
}
};
51 class NavigateThroughPosesActionTestFixture : public ::testing::Test
{
public:
54 static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "navigate_through_poses_action_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds( 20 ) );
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds( 10 ) );
config_->blackboard->set<bool>( "initial_pose_received", false );
std::vector<geometry_msgs::msg::PoseStamped> poses;
config_->blackboard->set<std::vector<geometry_msgs::msg::PoseStamped>>(
"goals", poses );
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::NavigateThroughPosesAction>(
name, "navigate_through_poses", config );
};
factory_->registerBuilder<nav2_behavior_tree::NavigateThroughPosesAction>(
"NavigateThroughPoses", builder );
}
89 static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
action_server_.reset( );
factory_.reset( );
}
void TearDown( ) override
{
tree_.reset( );
}
static std::shared_ptr<NavigateThroughPosesActionServer> action_server_;
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr NavigateThroughPosesActionTestFixture::node_ = nullptr;
std::shared_ptr<NavigateThroughPosesActionServer>
NavigateThroughPosesActionTestFixture::action_server_ = nullptr;
BT::NodeConfiguration * NavigateThroughPosesActionTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory> NavigateThroughPosesActionTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> NavigateThroughPosesActionTestFixture::tree_ = nullptr;
TEST_F( NavigateThroughPosesActionTestFixture, test_tick )
{
// create tree
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<NavigateThroughPoses goals="{goals}" />
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
std::vector<geometry_msgs::msg::PoseStamped> poses;
poses.resize( 1 );
poses[0].pose.position.x = -2.5;
poses[0].pose.orientation.x = 1.0;
config_->blackboard->set<std::vector<geometry_msgs::msg::PoseStamped>>( "goals", poses );
// tick until node succeeds
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
// goal should have reached our server
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( action_server_->getCurrentGoal( )->poses, poses );
// halt node so another goal can be sent
tree_->rootNode( )->halt( );
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::IDLE );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
// initialize action server and spin on new thread
NavigateThroughPosesActionTestFixture::action_server_ =
std::make_shared<NavigateThroughPosesActionServer>( );
std::thread server_thread( []( ) {
rclcpp::spin( NavigateThroughPosesActionTestFixture::action_server_ );
} );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
server_thread.join( );
return all_successful;
}
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "../../test_action_server.hpp"
#include "nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp"
30 class NavigateToPoseActionServer : public TestActionServer<nav2_msgs::action::NavigateToPose>
{
public:
33 NavigateToPoseActionServer( )
: TestActionServer( "navigate_to_pose" )
{}
protected:
void execute(
const typename std::shared_ptr<
rclcpp_action::ServerGoalHandle<nav2_msgs::action::NavigateToPose>> goal_handle )
override
{
const auto goal = goal_handle->get_goal( );
auto result = std::make_shared<nav2_msgs::action::NavigateToPose::Result>( );
goal_handle->succeed( result );
}
};
49 class NavigateToPoseActionTestFixture : public ::testing::Test
{
public:
52 static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "navigate_to_pose_action_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds( 20 ) );
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds( 10 ) );
config_->blackboard->set<bool>( "initial_pose_received", false );
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::NavigateToPoseAction>(
name, "navigate_to_pose", config );
};
factory_->registerBuilder<nav2_behavior_tree::NavigateToPoseAction>(
"NavigateToPose", builder );
}
84 static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
action_server_.reset( );
factory_.reset( );
}
void TearDown( ) override
{
tree_.reset( );
}
static std::shared_ptr<NavigateToPoseActionServer> action_server_;
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr NavigateToPoseActionTestFixture::node_ = nullptr;
std::shared_ptr<NavigateToPoseActionServer>
NavigateToPoseActionTestFixture::action_server_ = nullptr;
BT::NodeConfiguration * NavigateToPoseActionTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory> NavigateToPoseActionTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> NavigateToPoseActionTestFixture::tree_ = nullptr;
TEST_F( NavigateToPoseActionTestFixture, test_tick )
{
// create tree
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<NavigateToPose goal="{goal}" />
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
geometry_msgs::msg::PoseStamped pose;
// tick until node succeeds
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
// goal should have reached our server
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( action_server_->getCurrentGoal( )->pose, pose );
// halt node so another goal can be sent
tree_->rootNode( )->halt( );
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::IDLE );
// set new goal
pose.pose.position.x = -2.5;
pose.pose.orientation.x = 1.0;
config_->blackboard->set<geometry_msgs::msg::PoseStamped>( "goal", pose );
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
EXPECT_EQ( action_server_->getCurrentGoal( )->pose, pose );
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
// initialize action server and spin on new thread
NavigateToPoseActionTestFixture::action_server_ =
std::make_shared<NavigateToPoseActionServer>( );
std::thread server_thread( []( ) {
rclcpp::spin( NavigateToPoseActionTestFixture::action_server_ );
} );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
server_thread.join( );
return all_successful;
}
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Pablo Iñigo Blasco
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include "../../test_action_server.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "nav2_behavior_tree/plugins/action/planner_selector_node.hpp"
#include "nav_msgs/msg/path.hpp"
#include "std_msgs/msg/string.hpp"
28 class PlannerSelectorTestFixture : public ::testing::Test
{
public:
31 static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "planner_selector_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>( "node", node_ );
BT::NodeBuilder builder = []( const std::string & name, const BT::NodeConfiguration & config ) {
return std::make_unique<nav2_behavior_tree::PlannerSelector>( name, config );
};
factory_->registerBuilder<nav2_behavior_tree::PlannerSelector>( "PlannerSelector", builder );
}
50 static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
factory_.reset( );
}
void TearDown( ) override
{
tree_.reset( );
}
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr PlannerSelectorTestFixture::node_ = nullptr;
BT::NodeConfiguration * PlannerSelectorTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory> PlannerSelectorTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> PlannerSelectorTestFixture::tree_ = nullptr;
TEST_F( PlannerSelectorTestFixture, test_custom_topic )
{
// create tree
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector_custom_topic_name"/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
// tick until node succeeds
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
// check default value
std::string selected_planner_result;
config_->blackboard->get( "selected_planner", selected_planner_result );
EXPECT_EQ( selected_planner_result, "GridBased" );
std_msgs::msg::String selected_planner_cmd;
selected_planner_cmd.data = "RRT";
rclcpp::QoS qos( rclcpp::KeepLast( 1 ) );
qos.transient_local( ).reliable( );
auto planner_selector_pub =
node_->create_publisher<std_msgs::msg::String>( "planner_selector_custom_topic_name", qos );
// publish a few updates of the selected_planner
auto start = node_->now( );
while ( ( node_->now( ) - start ).seconds( ) < 0.5 ) {
tree_->rootNode( )->executeTick( );
planner_selector_pub->publish( selected_planner_cmd );
rclcpp::spin_some( node_ );
}
// check planner updated
config_->blackboard->get( "selected_planner", selected_planner_result );
EXPECT_EQ( "RRT", selected_planner_result );
}
TEST_F( PlannerSelectorTestFixture, test_default_topic )
{
// create tree
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased"/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
// tick until node succeeds
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
// check default value
std::string selected_planner_result;
config_->blackboard->get( "selected_planner", selected_planner_result );
EXPECT_EQ( selected_planner_result, "GridBased" );
std_msgs::msg::String selected_planner_cmd;
selected_planner_cmd.data = "RRT";
rclcpp::QoS qos( rclcpp::KeepLast( 1 ) );
qos.transient_local( ).reliable( );
auto planner_selector_pub =
node_->create_publisher<std_msgs::msg::String>( "planner_selector", qos );
// publish a few updates of the selected_planner
auto start = node_->now( );
while ( ( node_->now( ) - start ).seconds( ) < 0.5 ) {
tree_->rootNode( )->executeTick( );
planner_selector_pub->publish( selected_planner_cmd );
rclcpp::spin_some( node_ );
}
// check planner updated
config_->blackboard->get( "selected_planner", selected_planner_result );
EXPECT_EQ( "RRT", selected_planner_result );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include "behaviortree_cpp_v3/bt_factory.h"
#include "../../test_service.hpp"
#include "nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp"
26 class ReinitializeGlobalLocalizationService : public TestService<std_srvs::srv::Empty>
{
public:
29 ReinitializeGlobalLocalizationService( )
: TestService( "reinitialize_global_localization" )
{}
};
34 class ReinitializeGlobalLocalizationServiceTestFixture : public ::testing::Test
{
public:
37 static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "reinitialize_global_localization_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds( 20 ) );
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds( 10 ) );
config_->blackboard->set<bool>( "initial_pose_received", false );
factory_->registerNodeType<nav2_behavior_tree::ReinitializeGlobalLocalizationService>(
"ReinitializeGlobalLocalization" );
}
62 static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
server_.reset( );
factory_.reset( );
}
void TearDown( ) override
{
tree_.reset( );
}
static std::shared_ptr<ReinitializeGlobalLocalizationService> server_;
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr ReinitializeGlobalLocalizationServiceTestFixture::node_ = nullptr;
std::shared_ptr<ReinitializeGlobalLocalizationService>
ReinitializeGlobalLocalizationServiceTestFixture::server_ = nullptr;
BT::NodeConfiguration * ReinitializeGlobalLocalizationServiceTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory>
ReinitializeGlobalLocalizationServiceTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> ReinitializeGlobalLocalizationServiceTestFixture::tree_ = nullptr;
TEST_F( ReinitializeGlobalLocalizationServiceTestFixture, test_tick )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<ReinitializeGlobalLocalization service_name="reinitialize_global_localization"/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
EXPECT_EQ( tree_->rootNode( )->executeTick( ), BT::NodeStatus::SUCCESS );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
// initialize service and spin on new thread
ReinitializeGlobalLocalizationServiceTestFixture::server_ =
std::make_shared<ReinitializeGlobalLocalizationService>( );
std::thread server_thread( []( ) {
rclcpp::spin( ReinitializeGlobalLocalizationServiceTestFixture::server_ );
} );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
server_thread.join( );
return all_successful;
}
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2021 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include <vector>
#include "nav_msgs/msg/path.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "../../test_action_server.hpp"
#include "nav2_behavior_tree/plugins/action/remove_passed_goals_action.hpp"
#include "../../test_behavior_tree_fixture.hpp"
32 class RemovePassedGoalsTestFixture : public ::testing::Test
{
public:
35 static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "passed_goals_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
transform_handler_ = std::make_shared<nav2_behavior_tree::TransformHandler>( node_ );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
config_->blackboard->set<std::shared_ptr<tf2_ros::Buffer>>(
"tf_buffer",
transform_handler_->getBuffer( ) );
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::RemovePassedGoals>(
name, config );
};
factory_->registerBuilder<nav2_behavior_tree::RemovePassedGoals>(
"RemovePassedGoals", builder );
}
64 static void TearDownTestCase( )
{
transform_handler_->deactivate( );
delete config_;
config_ = nullptr;
transform_handler_.reset( );
node_.reset( );
factory_.reset( );
}
void TearDown( ) override
{
tree_.reset( );
}
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
static std::shared_ptr<nav2_behavior_tree::TransformHandler> transform_handler_;
};
rclcpp::Node::SharedPtr RemovePassedGoalsTestFixture::node_ = nullptr;
BT::NodeConfiguration * RemovePassedGoalsTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory> RemovePassedGoalsTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> RemovePassedGoalsTestFixture::tree_ = nullptr;
std::shared_ptr<nav2_behavior_tree::TransformHandler>
RemovePassedGoalsTestFixture::transform_handler_ = nullptr;
TEST_F( RemovePassedGoalsTestFixture, test_tick )
{
geometry_msgs::msg::Pose pose;
pose.position.x = 0.25;
pose.position.y = 0.0;
transform_handler_->activate( );
transform_handler_->waitForTransform( );
transform_handler_->updateRobotPose( pose );
// create tree
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<RemovePassedGoals radius="0.5" input_goals="{goals}" output_goals="{goals}"/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
// create new goal and set it on blackboard
std::vector<geometry_msgs::msg::PoseStamped> poses;
poses.resize( 4 );
poses[0].pose.position.x = 0.0;
poses[0].pose.position.y = 0.0;
poses[1].pose.position.x = 0.5;
poses[1].pose.position.y = 0.0;
poses[2].pose.position.x = 1.0;
poses[2].pose.position.y = 0.0;
poses[3].pose.position.x = 2.0;
poses[3].pose.position.y = 0.0;
config_->blackboard->set( "goals", poses );
// tick until node succeeds
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
// check that it removed the point in range
std::vector<geometry_msgs::msg::PoseStamped> output_poses;
config_->blackboard->get( "goals", output_poses );
EXPECT_EQ( output_poses.size( ), 2u );
EXPECT_EQ( output_poses[0], poses[2] );
EXPECT_EQ( output_poses[1], poses[3] );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
// Copyright ( c ) 2021 RoboTech Vision
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include "nav_msgs/msg/path.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "../../test_action_server.hpp"
#include "nav2_behavior_tree/plugins/action/smooth_path_action.hpp"
29 class SmoothPathActionServer : public TestActionServer<nav2_msgs::action::SmoothPath>
{
public:
32 SmoothPathActionServer( )
: TestActionServer( "smooth_path" )
{}
protected:
void execute(
const typename std::shared_ptr<
rclcpp_action::ServerGoalHandle<nav2_msgs::action::SmoothPath>> goal_handle )
override
{
const auto goal = goal_handle->get_goal( );
auto result = std::make_shared<nav2_msgs::action::SmoothPath::Result>( );
goal_handle->succeed( result );
}
};
48 class SmoothPathActionTestFixture : public ::testing::Test
{
public:
51 static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "smooth_path_action_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds( 20 ) );
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds( 10 ) );
config_->blackboard->set<bool>( "initial_pose_received", false );
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::SmoothPathAction>(
name, "smooth_path", config );
};
factory_->registerBuilder<nav2_behavior_tree::SmoothPathAction>(
"SmoothPath", builder );
}
83 static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
action_server_.reset( );
factory_.reset( );
}
void TearDown( ) override
{
tree_.reset( );
}
static std::shared_ptr<SmoothPathActionServer> action_server_;
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr SmoothPathActionTestFixture::node_ = nullptr;
std::shared_ptr<SmoothPathActionServer>
SmoothPathActionTestFixture::action_server_ = nullptr;
BT::NodeConfiguration * SmoothPathActionTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory> SmoothPathActionTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> SmoothPathActionTestFixture::tree_ = nullptr;
TEST_F( SmoothPathActionTestFixture, test_tick )
{
// create tree
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<SmoothPath unsmoothed_path="{unsmoothed_path}" />
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
nav_msgs::msg::Path path;
// tick until node succeeds
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
// goal should have reached our server
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( action_server_->getCurrentGoal( )->path, path );
// halt node so another goal can be sent
tree_->rootNode( )->halt( );
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::IDLE );
// set new goal
geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x = -2.5;
pose.pose.orientation.x = 1.0;
path.poses.push_back( pose );
config_->blackboard->set<nav_msgs::msg::Path>( "unsmoothed_path", path );
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
nav_msgs::msg::Path path_empty;
EXPECT_NE( path_empty, path );
EXPECT_EQ( action_server_->getCurrentGoal( )->path, path );
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
// initialize action server and spin on new thread
SmoothPathActionTestFixture::action_server_ =
std::make_shared<SmoothPathActionServer>( );
std::thread server_thread( []( ) {
rclcpp::spin( SmoothPathActionTestFixture::action_server_ );
} );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
server_thread.join( );
return all_successful;
}
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include "behaviortree_cpp_v3/bt_factory.h"
#include "../../test_action_server.hpp"
#include "nav2_behavior_tree/plugins/action/spin_action.hpp"
26 class SpinActionServer : public TestActionServer<nav2_msgs::action::Spin>
{
public:
29 SpinActionServer( )
: TestActionServer( "spin" )
{}
protected:
void execute(
const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin>>
goal_handle )
override
{
nav2_msgs::action::Spin::Result::SharedPtr result =
std::make_shared<nav2_msgs::action::Spin::Result>( );
bool return_success = getReturnSuccess( );
if ( return_success ) {
goal_handle->succeed( result );
} else {
goal_handle->abort( result );
}
}
};
class SpinActionTestFixture : public ::testing::Test
{
public:
static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "spin_action_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds( 20 ) );
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds( 10 ) );
config_->blackboard->set<bool>( "initial_pose_received", false );
config_->blackboard->set<int>( "number_recoveries", 0 );
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::SpinAction>(
name, "spin", config );
};
factory_->registerBuilder<nav2_behavior_tree::SpinAction>( "Spin", builder );
}
static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
action_server_.reset( );
factory_.reset( );
}
void SetUp( ) override
{
config_->blackboard->set( "number_recoveries", 0 );
}
void TearDown( ) override
{
tree_.reset( );
}
static std::shared_ptr<SpinActionServer> action_server_;
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr SpinActionTestFixture::node_ = nullptr;
std::shared_ptr<SpinActionServer> SpinActionTestFixture::action_server_ = nullptr;
BT::NodeConfiguration * SpinActionTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory> SpinActionTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> SpinActionTestFixture::tree_ = nullptr;
TEST_F( SpinActionTestFixture, test_ports )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Spin server_name="spin"/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
EXPECT_EQ( tree_->rootNode( )->getInput<double>( "spin_dist" ), 1.57 );
xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Spin spin_dist="3.14" />
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
EXPECT_EQ( tree_->rootNode( )->getInput<double>( "spin_dist" ), 3.14 );
}
TEST_F( SpinActionTestFixture, test_tick )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Spin spin_dist="3.14" />
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
EXPECT_EQ( config_->blackboard->get<int>( "number_recoveries" ), 0 );
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( config_->blackboard->get<int>( "number_recoveries" ), 1 );
EXPECT_EQ( action_server_->getCurrentGoal( )->target_yaw, 3.14f );
}
TEST_F( SpinActionTestFixture, test_failure )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Spin spin_dist="3.14" />
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
action_server_->setReturnSuccess( false );
EXPECT_EQ( config_->blackboard->get<int>( "number_recoveries" ), 0 );
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS &&
tree_->rootNode( )->status( ) != BT::NodeStatus::FAILURE )
{
tree_->rootNode( )->executeTick( );
}
std::cout << tree_->rootNode( )->status( );
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::FAILURE );
EXPECT_EQ( config_->blackboard->get<int>( "number_recoveries" ), 1 );
EXPECT_EQ( action_server_->getCurrentGoal( )->target_yaw, 3.14f );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
// initialize action server and spin on new thread
SpinActionTestFixture::action_server_ = std::make_shared<SpinActionServer>( );
std::thread server_thread( []( ) {
rclcpp::spin( SpinActionTestFixture::action_server_ );
} );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
server_thread.join( );
return all_successful;
}
// Copyright ( c ) 2022 Neobotix GmbH
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include "behaviortree_cpp_v3/bt_factory.h"
#include "../../test_action_server.hpp"
#include "nav2_behavior_tree/plugins/action/spin_cancel_node.hpp"
#include "lifecycle_msgs/srv/change_state.hpp"
26 class CancelSpinServer : public TestActionServer<nav2_msgs::action::Spin>
{
public:
29 CancelSpinServer( )
: TestActionServer( "spin" )
{}
protected:
34 void execute(
const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Spin>>
goal_handle )
{
while ( !goal_handle->is_canceling( ) ) {
// Spining here until goal cancels
std::this_thread::sleep_for( std::chrono::milliseconds( 15 ) );
}
}
};
45 class CancelSpinActionTestFixture : public ::testing::Test
{
public:
48 static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "cancel_spin_action_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds( 20 ) );
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds( 10 ) );
client_ = rclcpp_action::create_client<nav2_msgs::action::Spin>(
node_, "spin" );
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::SpinCancel>(
name, "spin", config );
};
factory_->registerBuilder<nav2_behavior_tree::SpinCancel>( "CancelSpin", builder );
}
80 static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
action_server_.reset( );
client_.reset( );
factory_.reset( );
}
void TearDown( ) override
{
tree_.reset( );
}
static std::shared_ptr<CancelSpinServer> action_server_;
96 static std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::Spin>> client_;
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr CancelSpinActionTestFixture::node_ = nullptr;
std::shared_ptr<CancelSpinServer>
CancelSpinActionTestFixture::action_server_ = nullptr;
std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::Spin>>
CancelSpinActionTestFixture::client_ = nullptr;
BT::NodeConfiguration * CancelSpinActionTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory>
CancelSpinActionTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> CancelSpinActionTestFixture::tree_ = nullptr;
TEST_F( CancelSpinActionTestFixture, test_ports )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<CancelSpin name="SpinCancel"/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
auto send_goal_options = rclcpp_action::Client<nav2_msgs::action::Spin>::SendGoalOptions( );
// Creating a dummy goal_msg
auto goal_msg = nav2_msgs::action::Spin::Goal( );
// Setting target yaw
goal_msg.target_yaw = 1.57;
// Spining for server and sending a goal
client_->wait_for_action_server( );
client_->async_send_goal( goal_msg, send_goal_options );
// Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class
std::this_thread::sleep_for( std::chrono::milliseconds( 15 ) );
// Executing tick
tree_->rootNode( )->executeTick( );
// BT node should return success, once when the goal is cancelled
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
// Adding another test case to check if the goal is infact cancelling
EXPECT_EQ( action_server_->isGoalCancelled( ), true );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
// initialize action server and spin on new thread
CancelSpinActionTestFixture::action_server_ = std::make_shared<CancelSpinServer>( );
std::thread server_thread( []( ) {
rclcpp::spin( CancelSpinActionTestFixture::action_server_ );
} );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
server_thread.join( );
return all_successful;
}
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Francisco Martin Rico
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include "nav_msgs/msg/path.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "../../test_action_server.hpp"
#include "nav2_behavior_tree/plugins/action/truncate_path_action.hpp"
31 class TruncatePathTestFixture : public ::testing::Test
{
public:
34 static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "change_goal_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::TruncatePath>(
name, config );
};
factory_->registerBuilder<nav2_behavior_tree::TruncatePath>(
"TruncatePath", builder );
}
59 static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
factory_.reset( );
}
void TearDown( ) override
{
tree_.reset( );
}
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr TruncatePathTestFixture::node_ = nullptr;
BT::NodeConfiguration * TruncatePathTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory> TruncatePathTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> TruncatePathTestFixture::tree_ = nullptr;
TEST_F( TruncatePathTestFixture, test_tick )
{
// create tree
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<TruncatePath distance="1.0" input_path="{path}" output_path="{truncated_path}"/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
// create new goal and set it on blackboard
nav_msgs::msg::Path path;
path.header.stamp = node_->now( );
geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( 0.0 );
path.poses.push_back( pose );
pose.pose.position.x = 0.5;
pose.pose.position.y = 0.0;
path.poses.push_back( pose );
pose.pose.position.x = 0.9;
pose.pose.position.y = 0.0;
path.poses.push_back( pose );
pose.pose.position.x = 1.5;
pose.pose.position.y = 0.5;
path.poses.push_back( pose );
EXPECT_EQ( path.poses.size( ), 4u );
config_->blackboard->set( "path", path );
// tick until node succeeds
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
nav_msgs::msg::Path truncated_path;
config_->blackboard->get( "truncated_path", truncated_path );
EXPECT_NE( path, truncated_path );
EXPECT_EQ( truncated_path.poses.size( ), 2u );
double r, p, y;
tf2::Quaternion q;
tf2::fromMsg( truncated_path.poses.back( ).pose.orientation, q );
tf2::Matrix3x3( q ).getRPY( r, p, y );
EXPECT_NEAR( y, 0.463, 0.001 );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
// Copyright ( c ) 2021 RoboTech Vision
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include "nav_msgs/msg/path.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "../../test_behavior_tree_fixture.hpp"
#include "nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp"
30 class TruncatePathLocalTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
{
public:
void SetUp( ) override
{
bt_node_ = std::make_shared<nav2_behavior_tree::TruncatePathLocal>(
"truncate_path_local", *config_ );
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::TruncatePathLocal>(
name, config );
};
try {
factory_->registerBuilder<nav2_behavior_tree::TruncatePathLocal>(
"TruncatePathLocal", builder );
} catch ( BT::BehaviorTreeException const & ) {
// ignoring multiple registrations of TruncatePathLocal
}
}
void TearDown( ) override
{
bt_node_.reset( );
tree_.reset( );
}
static geometry_msgs::msg::PoseStamped poseMsg( double x, double y, double orientation )
{
geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x = x;
pose.pose.position.y = y;
pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( orientation );
return pose;
}
nav_msgs::msg::Path createLoopCrossingTestPath( )
{
nav_msgs::msg::Path path;
path.header.stamp = node_->now( );
path.header.frame_id = "map";
// this is a loop to make it harder for robot to find the proper closest pose
path.poses.push_back( poseMsg( -0.3, -1.2, -M_PI * 3 / 2 ) );
// the position is closest to robot but orientation is different
path.poses.push_back( poseMsg( -0.3, 0.0, -M_PI * 3 / 2 ) );
path.poses.push_back( poseMsg( -0.5, 1.0, -M_PI ) );
path.poses.push_back( poseMsg( -1.5, 1.0, -M_PI / 2 ) );
path.poses.push_back( poseMsg( -1.5, 0.0, 0.0 ) );
// this is the correct path section for the first match
path.poses.push_back( poseMsg( -0.5, 0.0, 0.0 ) );
path.poses.push_back( poseMsg( 0.4, 0.0, 0.0 ) );
path.poses.push_back( poseMsg( 1.5, 0.0, 0.0 ) );
// this is a loop to make it harder for robot to find the proper closest pose
path.poses.push_back( poseMsg( 1.5, 1.0, M_PI / 2 ) );
path.poses.push_back( poseMsg( 0.5, 1.0, M_PI ) );
// the position is closest to robot but orientation is different
path.poses.push_back( poseMsg( 0.3, 0.0, M_PI * 3 / 2 ) );
path.poses.push_back( poseMsg( 0.3, -1.0, M_PI * 3 / 2 ) );
return path;
}
protected:
static std::shared_ptr<nav2_behavior_tree::TruncatePathLocal> bt_node_;
static std::shared_ptr<BT::Tree> tree_;
};
std::shared_ptr<nav2_behavior_tree::TruncatePathLocal> TruncatePathLocalTestFixture::bt_node_ =
nullptr;
std::shared_ptr<BT::Tree> TruncatePathLocalTestFixture::tree_ = nullptr;
TEST_F( TruncatePathLocalTestFixture, test_tick )
{
// create tree
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<TruncatePathLocal
distance_forward="2.0"
distance_backward="1.0"
robot_frame="base_link"
transform_tolerance="0.2"
angular_distance_weight="0.2"
pose="{pose}"
input_path="{path}"
output_path="{truncated_path}"
/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
// create path and set it on blackboard
nav_msgs::msg::Path path = createLoopCrossingTestPath( );
EXPECT_EQ( path.poses.size( ), 12u );
config_->blackboard->set( "path", path );
// tick until node succeeds
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS &&
tree_->rootNode( )->status( ) != BT::NodeStatus::FAILURE )
{
tree_->rootNode( )->executeTick( );
}
nav_msgs::msg::Path truncated_path;
config_->blackboard->get( "truncated_path", truncated_path );
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
EXPECT_NE( path, truncated_path );
ASSERT_GE( truncated_path.poses.size( ), 1u );
EXPECT_EQ( truncated_path.poses.size( ), 3u );
EXPECT_EQ( truncated_path.poses.front( ).pose.position.x, -0.5 );
EXPECT_EQ( truncated_path.poses.front( ).pose.position.y, 0.0 );
EXPECT_EQ( truncated_path.poses.back( ).pose.position.x, 1.5 );
EXPECT_EQ( truncated_path.poses.back( ).pose.position.y, 0.0 );
/////////////////////////////////////////
// should match the first loop crossing
config_->blackboard->set( "pose", poseMsg( 0.0, 0.0, M_PI / 2 ) );
tree_->haltTree( );
// tick until node succeeds
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS &&
tree_->rootNode( )->status( ) != BT::NodeStatus::FAILURE )
{
tree_->rootNode( )->executeTick( );
}
config_->blackboard->get( "truncated_path", truncated_path );
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
EXPECT_NE( path, truncated_path );
ASSERT_GE( truncated_path.poses.size( ), 1u );
EXPECT_EQ( truncated_path.poses.size( ), 2u );
EXPECT_EQ( truncated_path.poses.front( ).pose.position.x, -0.3 );
EXPECT_EQ( truncated_path.poses.front( ).pose.position.y, 0.0 );
EXPECT_EQ( truncated_path.poses.back( ).pose.position.x, -0.5 );
EXPECT_EQ( truncated_path.poses.back( ).pose.position.y, 1.0 );
/////////////////////////////////////////
// should match the last loop crossing
config_->blackboard->set( "pose", poseMsg( 0.0, 0.0, -M_PI / 2 ) );
tree_->haltTree( );
// tick until node succeeds
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS &&
tree_->rootNode( )->status( ) != BT::NodeStatus::FAILURE )
{
tree_->rootNode( )->executeTick( );
}
config_->blackboard->get( "truncated_path", truncated_path );
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
EXPECT_NE( path, truncated_path );
ASSERT_GE( truncated_path.poses.size( ), 1u );
EXPECT_EQ( truncated_path.poses.size( ), 2u );
EXPECT_EQ( truncated_path.poses.front( ).pose.position.x, 0.3 );
EXPECT_EQ( truncated_path.poses.front( ).pose.position.y, 0.0 );
EXPECT_EQ( truncated_path.poses.back( ).pose.position.x, 0.3 );
EXPECT_EQ( truncated_path.poses.back( ).pose.position.y, -1.0 );
SUCCEED( );
}
TEST_F( TruncatePathLocalTestFixture, test_success_on_empty_path )
{
// create tree
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<TruncatePathLocal
distance_forward="2.0"
distance_backward="1.0"
robot_frame="base_link"
transform_tolerance="0.2"
angular_distance_weight="0.2"
pose="{pose}"
input_path="{path}"
output_path="{truncated_path}"
max_robot_pose_search_dist="infinity"
/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
// create path and set it on blackboard
nav_msgs::msg::Path path;
path.header.stamp = node_->now( );
path.header.frame_id = "map";
config_->blackboard->set( "path", path );
// tick until node succeeds
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS &&
tree_->rootNode( )->status( ) != BT::NodeStatus::FAILURE )
{
tree_->rootNode( )->executeTick( );
}
nav_msgs::msg::Path truncated_path;
config_->blackboard->get( "truncated_path", truncated_path );
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( path, truncated_path );
SUCCEED( );
}
TEST_F( TruncatePathLocalTestFixture, test_failure_on_no_pose )
{
// create tree
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<TruncatePathLocal
distance_forward="2.0"
distance_backward="1.0"
transform_tolerance="0.2"
angular_distance_weight="0.2"
robot_frame="{robot_frame}"
input_path="{path}"
output_path="{truncated_path}"
max_robot_pose_search_dist="infinity"
/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
// create path and set it on blackboard
nav_msgs::msg::Path path;
path.header.stamp = node_->now( );
path.header.frame_id = "map";
config_->blackboard->set( "path", path );
// tick until node succeeds
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS &&
tree_->rootNode( )->status( ) != BT::NodeStatus::FAILURE )
{
tree_->rootNode( )->executeTick( );
}
nav_msgs::msg::Path truncated_path;
config_->blackboard->get( "truncated_path", truncated_path );
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::FAILURE );
SUCCEED( );
}
TEST_F( TruncatePathLocalTestFixture, test_failure_on_invalid_robot_frame )
{
// create tree
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<TruncatePathLocal
distance_forward="2.0"
distance_backward="1.0"
transform_tolerance="0.2"
robot_frame="invalid_frame"
angular_distance_weight="0.2"
input_path="{path}"
output_path="{truncated_path}"
max_robot_pose_search_dist="infinity"
/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
// create new goal and set it on blackboard
nav_msgs::msg::Path path = createLoopCrossingTestPath( );
EXPECT_EQ( path.poses.size( ), 12u );
config_->blackboard->set( "path", path );
// tick until node succeeds
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS &&
tree_->rootNode( )->status( ) != BT::NodeStatus::FAILURE )
{
tree_->rootNode( )->executeTick( );
}
nav_msgs::msg::Path truncated_path;
config_->blackboard->get( "truncated_path", truncated_path );
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::FAILURE );
SUCCEED( );
}
TEST_F( TruncatePathLocalTestFixture, test_path_pruning )
{
// create tree
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<TruncatePathLocal
distance_forward="2.0"
distance_backward="1.0"
robot_frame="base_link"
transform_tolerance="0.2"
angular_distance_weight="0.0"
pose="{pose}"
input_path="{path}"
output_path="{truncated_path}"
max_robot_pose_search_dist="3.0"
/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
// create path and set it on blackboard
nav_msgs::msg::Path path = createLoopCrossingTestPath( );
nav_msgs::msg::Path truncated_path;
config_->blackboard->set( "path", path );
/////////////////////////////////////////
// should match the first loop crossing
config_->blackboard->set( "pose", poseMsg( 0.0, 0.0, 0.0 ) );
// tick until node succeeds
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS &&
tree_->rootNode( )->status( ) != BT::NodeStatus::FAILURE )
{
tree_->rootNode( )->executeTick( );
}
config_->blackboard->get( "truncated_path", truncated_path );
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
EXPECT_NE( path, truncated_path );
ASSERT_GE( truncated_path.poses.size( ), 1u );
EXPECT_EQ( truncated_path.poses.size( ), 2u );
EXPECT_EQ( truncated_path.poses.front( ).pose.position.x, -0.3 );
EXPECT_EQ( truncated_path.poses.front( ).pose.position.y, 0.0 );
EXPECT_EQ( truncated_path.poses.back( ).pose.position.x, -0.5 );
EXPECT_EQ( truncated_path.poses.back( ).pose.position.y, 1.0 );
/////////////////////////////////////////
// move along the path to leave the first loop crossing behind
config_->blackboard->set( "pose", poseMsg( -1.5, 1.0, 0.0 ) );
// tick until node succeeds
tree_->haltTree( );
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS &&
tree_->rootNode( )->status( ) != BT::NodeStatus::FAILURE )
{
tree_->rootNode( )->executeTick( );
}
// this truncated_path is not interesting, let's proceed to the second loop crossing
/////////////////////////////////////////
// should match the second loop crossing
config_->blackboard->set( "pose", poseMsg( 0.0, 0.0, 0.0 ) );
// tick until node succeeds
tree_->haltTree( );
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS &&
tree_->rootNode( )->status( ) != BT::NodeStatus::FAILURE )
{
tree_->rootNode( )->executeTick( );
}
config_->blackboard->get( "truncated_path", truncated_path );
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
EXPECT_NE( path, truncated_path );
ASSERT_GE( truncated_path.poses.size( ), 1u );
EXPECT_EQ( truncated_path.poses.size( ), 3u );
EXPECT_EQ( truncated_path.poses.front( ).pose.position.x, -0.5 );
EXPECT_EQ( truncated_path.poses.front( ).pose.position.y, 0.0 );
EXPECT_EQ( truncated_path.poses.back( ).pose.position.x, 1.5 );
EXPECT_EQ( truncated_path.poses.back( ).pose.position.y, 0.0 );
/////////////////////////////////////////
// move along the path to leave the second loop crossing behind
config_->blackboard->set( "pose", poseMsg( 1.5, 1.0, 0.0 ) );
// tick until node succeeds
tree_->haltTree( );
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS &&
tree_->rootNode( )->status( ) != BT::NodeStatus::FAILURE )
{
tree_->rootNode( )->executeTick( );
}
// this truncated_path is not interesting, let's proceed to the last loop crossing
/////////////////////////////////////////
// should match the last loop crossing
config_->blackboard->set( "pose", poseMsg( 0.0, 0.0, 0.0 ) );
// tick until node succeeds
tree_->haltTree( );
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS &&
tree_->rootNode( )->status( ) != BT::NodeStatus::FAILURE )
{
tree_->rootNode( )->executeTick( );
}
config_->blackboard->get( "truncated_path", truncated_path );
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
EXPECT_NE( path, truncated_path );
ASSERT_GE( truncated_path.poses.size( ), 1u );
EXPECT_EQ( truncated_path.poses.size( ), 2u );
EXPECT_EQ( truncated_path.poses.front( ).pose.position.x, 0.3 );
EXPECT_EQ( truncated_path.poses.front( ).pose.position.y, 0.0 );
EXPECT_EQ( truncated_path.poses.back( ).pose.position.x, 0.3 );
EXPECT_EQ( truncated_path.poses.back( ).pose.position.y, -1.0 );
SUCCEED( );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include "behaviortree_cpp_v3/bt_factory.h"
#include "../../test_action_server.hpp"
#include "nav2_behavior_tree/plugins/action/wait_action.hpp"
26 class WaitActionServer : public TestActionServer<nav2_msgs::action::Wait>
{
public:
29 WaitActionServer( )
: TestActionServer( "wait" )
{}
protected:
void execute(
const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Wait>> )
override
{}
};
40 class WaitActionTestFixture : public ::testing::Test
{
public:
43 static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "wait_action_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds( 20 ) );
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds( 10 ) );
config_->blackboard->set<bool>( "initial_pose_received", false );
config_->blackboard->set<int>( "number_recoveries", 0 );
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::WaitAction>(
name, "wait", config );
};
factory_->registerBuilder<nav2_behavior_tree::WaitAction>( "Wait", builder );
}
75 static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
action_server_.reset( );
factory_.reset( );
}
void SetUp( ) override
{
config_->blackboard->set( "number_recoveries", 0 );
}
void TearDown( ) override
{
tree_.reset( );
}
static std::shared_ptr<WaitActionServer> action_server_;
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr WaitActionTestFixture::node_ = nullptr;
std::shared_ptr<WaitActionServer> WaitActionTestFixture::action_server_ = nullptr;
BT::NodeConfiguration * WaitActionTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory> WaitActionTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> WaitActionTestFixture::tree_ = nullptr;
TEST_F( WaitActionTestFixture, test_ports )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Wait />
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
EXPECT_EQ( tree_->rootNode( )->getInput<int>( "wait_duration" ), 1 );
xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Wait wait_duration="10" />
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
EXPECT_EQ( tree_->rootNode( )->getInput<int>( "wait_duration" ), 10 );
}
TEST_F( WaitActionTestFixture, test_tick )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Wait wait_duration="-5"/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
EXPECT_EQ( config_->blackboard->get<int>( "number_recoveries" ), 0 );
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( config_->blackboard->get<int>( "number_recoveries" ), 1 );
EXPECT_EQ( rclcpp::Duration( action_server_->getCurrentGoal( )->time ).seconds( ), 5.0 );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
// initialize action server and spin on new thread
WaitActionTestFixture::action_server_ = std::make_shared<WaitActionServer>( );
std::thread server_thread( []( ) {
rclcpp::spin( WaitActionTestFixture::action_server_ );
} );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
server_thread.join( );
return all_successful;
}
// Copyright ( c ) 2022 Neobotix GmbH
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include "behaviortree_cpp_v3/bt_factory.h"
#include "../../test_action_server.hpp"
#include "nav2_behavior_tree/plugins/action/wait_cancel_node.hpp"
#include "lifecycle_msgs/srv/change_state.hpp"
26 class CancelWaitServer : public TestActionServer<nav2_msgs::action::Wait>
{
public:
29 CancelWaitServer( )
: TestActionServer( "wait" )
{}
protected:
34 void execute(
const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<nav2_msgs::action::Wait>>
goal_handle )
{
while ( !goal_handle->is_canceling( ) ) {
// waiting here until goal cancels
std::this_thread::sleep_for( std::chrono::milliseconds( 15 ) );
}
}
};
45 class CancelWaitActionTestFixture : public ::testing::Test
{
public:
48 static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "cancel_wait_action_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds( 20 ) );
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds( 10 ) );
client_ = rclcpp_action::create_client<nav2_msgs::action::Wait>(
node_, "wait" );
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::WaitCancel>(
name, "wait", config );
};
factory_->registerBuilder<nav2_behavior_tree::WaitCancel>( "CancelWait", builder );
}
80 static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
action_server_.reset( );
client_.reset( );
factory_.reset( );
}
void TearDown( ) override
{
tree_.reset( );
}
static std::shared_ptr<CancelWaitServer> action_server_;
96 static std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::Wait>> client_;
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr CancelWaitActionTestFixture::node_ = nullptr;
std::shared_ptr<CancelWaitServer>
CancelWaitActionTestFixture::action_server_ = nullptr;
std::shared_ptr<rclcpp_action::Client<nav2_msgs::action::Wait>>
CancelWaitActionTestFixture::client_ = nullptr;
BT::NodeConfiguration * CancelWaitActionTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory>
CancelWaitActionTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> CancelWaitActionTestFixture::tree_ = nullptr;
TEST_F( CancelWaitActionTestFixture, test_ports )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<CancelWait name="WaitCancel"/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
auto send_goal_options = rclcpp_action::Client<nav2_msgs::action::Wait>::SendGoalOptions( );
// Creating a dummy goal_msg
auto goal_msg = nav2_msgs::action::Wait::Goal( );
// Setting a waiting time for 5 Seconds.
goal_msg.time.sec = 5;
// Waiting for server and sending a goal
client_->wait_for_action_server( );
client_->async_send_goal( goal_msg, send_goal_options );
// Adding a sleep so that the goal is indeed older than 10ms as described in our abstract class
std::this_thread::sleep_for( std::chrono::milliseconds( 15 ) );
// Executing tick
tree_->rootNode( )->executeTick( );
// BT node should return success, once when the goal is cancelled
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
// Adding another test case to check if the goal is infact cancelling
EXPECT_EQ( action_server_->isGoalCancelled( ), true );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
// initialize action server and spin on new thread
CancelWaitActionTestFixture::action_server_ = std::make_shared<CancelWaitServer>( );
std::thread server_thread( []( ) {
rclcpp::spin( CancelWaitActionTestFixture::action_server_ );
} );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
server_thread.join( );
return all_successful;
}
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/robot_utils.hpp"
#include "../../test_behavior_tree_fixture.hpp"
#include "nav2_behavior_tree/plugins/condition/distance_traveled_condition.hpp"
25 class DistanceTraveledConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
{
public:
28 void SetUp( )
{
bt_node_ = std::make_shared<nav2_behavior_tree::DistanceTraveledCondition>(
"distance_traveled", *config_ );
}
34 void TearDown( )
{
bt_node_.reset( );
}
protected:
40 static std::shared_ptr<nav2_behavior_tree::DistanceTraveledCondition> bt_node_;
};
std::shared_ptr<nav2_behavior_tree::DistanceTraveledCondition>
DistanceTraveledConditionTestFixture::bt_node_ = nullptr;
46 TEST_F( DistanceTraveledConditionTestFixture, test_behavior )
{
EXPECT_EQ( bt_node_->status( ), BT::NodeStatus::IDLE );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::FAILURE );
geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.orientation.w = 1;
double traveled = 0;
for ( int i = 1; i <= 20; i++ ) {
pose.pose.position.x = i * 0.51;
transform_handler_->updateRobotPose( pose.pose );
// Wait for transforms to actually update
// updated pose is i * 0.51
// we wait for the traveled distance to reach a value > i * 0.5
// we can assume the current transform has been updated at this point
while ( traveled < i * 0.5 ) {
if ( nav2_util::getCurrentPose( pose, *transform_handler_->getBuffer( ) ) ) {
traveled = pose.pose.position.x;
}
}
if ( i % 2 ) {
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::FAILURE );
} else {
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
}
}
}
79 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2021 Joshua Wallace
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/robot_utils.hpp"
#include "../../test_behavior_tree_fixture.hpp"
#include "nav2_behavior_tree/plugins/condition/globally_updated_goal_condition.hpp"
25 class GloballyUpdatedGoalConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
{
public:
28 void SetUp( )
{
bt_node_ = std::make_shared<nav2_behavior_tree::GloballyUpdatedGoalCondition>(
"globally_updated_goal", *config_ );
}
34 void TearDown( )
{
bt_node_.reset( );
}
protected:
40 static std::shared_ptr<nav2_behavior_tree::GloballyUpdatedGoalCondition> bt_node_;
};
std::shared_ptr<nav2_behavior_tree::GloballyUpdatedGoalCondition>
GloballyUpdatedGoalConditionTestFixture::bt_node_ = nullptr;
46 TEST_F( GloballyUpdatedGoalConditionTestFixture, test_behavior )
{
geometry_msgs::msg::PoseStamped goal;
config_->blackboard->set( "goal", goal );
EXPECT_EQ( bt_node_->status( ), BT::NodeStatus::IDLE );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::FAILURE );
goal.pose.position.x = 1.0;
config_->blackboard->set( "goal", goal );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::FAILURE );
}
60 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <chrono>
#include <memory>
#include <set>
#include <string>
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "../../test_behavior_tree_fixture.hpp"
#include "nav2_behavior_tree/plugins/condition/goal_reached_condition.hpp"
using namespace std::chrono; // NOLINT
using namespace std::chrono_literals; // NOLINT
30 class GoalReachedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
{
public:
33 void SetUp( )
{
node_->declare_parameter( "transform_tolerance", rclcpp::ParameterValue{0.1} );
geometry_msgs::msg::PoseStamped goal;
goal.header.stamp = node_->now( );
goal.header.frame_id = "map";
goal.pose.position.x = 1.0;
goal.pose.position.y = 1.0;
config_->blackboard->set( "goal", goal );
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<GoalReached goal="{goal}" />
</BehaviorTree>
</root> )";
51
factory_->registerNodeType<nav2_behavior_tree::GoalReachedCondition>( "GoalReached" );
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
}
void TearDown( )
57 {
tree_.reset( );
}
protected:
62 static std::shared_ptr<BT::Tree> tree_;
};
std::shared_ptr<BT::Tree> GoalReachedConditionTestFixture::tree_ = nullptr;
TEST_F( GoalReachedConditionTestFixture, test_behavior )
{
EXPECT_EQ( tree_->tickRoot( ), BT::NodeStatus::FAILURE );
geometry_msgs::msg::Pose pose;
pose.position.x = 0.0;
pose.position.y = 0.0;
transform_handler_->updateRobotPose( pose );
std::this_thread::sleep_for( 500ms );
EXPECT_EQ( tree_->tickRoot( ), BT::NodeStatus::FAILURE );
pose.position.x = 0.5;
pose.position.y = 0.5;
transform_handler_->updateRobotPose( pose );
std::this_thread::sleep_for( 500ms );
EXPECT_EQ( tree_->tickRoot( ), BT::NodeStatus::FAILURE );
pose.position.x = 0.9;
pose.position.y = 0.9;
transform_handler_->updateRobotPose( pose );
std::this_thread::sleep_for( 500ms );
EXPECT_EQ( tree_->tickRoot( ), BT::NodeStatus::SUCCESS );
pose.position.x = 1.0;
pose.position.y = 1.0;
92 transform_handler_->updateRobotPose( pose );
std::this_thread::sleep_for( 500ms );
EXPECT_EQ( tree_->tickRoot( ), BT::NodeStatus::SUCCESS );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/robot_utils.hpp"
#include "../../test_behavior_tree_fixture.hpp"
#include "nav2_behavior_tree/plugins/condition/goal_updated_condition.hpp"
26 class GoalUpdatedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
{
public:
29 void SetUp( )
{
bt_node_ = std::make_shared<nav2_behavior_tree::GoalUpdatedCondition>(
"goal_updated", *config_ );
}
35 void TearDown( )
{
bt_node_.reset( );
}
protected:
41 static std::shared_ptr<nav2_behavior_tree::GoalUpdatedCondition> bt_node_;
};
std::shared_ptr<nav2_behavior_tree::GoalUpdatedCondition>
GoalUpdatedConditionTestFixture::bt_node_ = nullptr;
47 TEST_F( GoalUpdatedConditionTestFixture, test_behavior )
{
geometry_msgs::msg::PoseStamped goal;
config_->blackboard->set( "goal", goal );
EXPECT_EQ( bt_node_->status( ), BT::NodeStatus::IDLE );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::FAILURE );
goal.pose.position.x = 1.0;
config_->blackboard->set( "goal", goal );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::FAILURE );
}
61 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include "../../test_behavior_tree_fixture.hpp"
#include "nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp"
24 class TestNode : public BT::SyncActionNode
{
public:
27 TestNode( const std::string & name, const BT::NodeConfiguration & config )
: SyncActionNode( name, config )
{}
31 BT::NodeStatus tick( )
{
return BT::NodeStatus::SUCCESS;
}
36 static BT::PortsList providedPorts( )
{
return {};
}
};
42 class InitialPoseReceivedConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
{
public:
45 void SetUp( )
{
test_node_ = std::make_shared<TestNode>( "TestNode", *config_ );
}
50 void TearDown( )
{
test_node_.reset( );
}
protected:
56 static std::shared_ptr<TestNode> test_node_;
};
std::shared_ptr<TestNode> InitialPoseReceivedConditionTestFixture::test_node_ = nullptr;
61 TEST_F( InitialPoseReceivedConditionTestFixture, test_behavior )
{
EXPECT_EQ( nav2_behavior_tree::initialPoseReceived( *test_node_ ), BT::NodeStatus::FAILURE );
config_->blackboard->set( "initial_pose_received", true );
EXPECT_EQ( nav2_behavior_tree::initialPoseReceived( *test_node_ ), BT::NodeStatus::SUCCESS );
}
68 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2020 Sarthak Mittal
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include <chrono>
#include "sensor_msgs/msg/battery_state.hpp"
#include "../../test_behavior_tree_fixture.hpp"
#include "nav2_behavior_tree/plugins/condition/is_battery_low_condition.hpp"
27 class IsBatteryLowConditionTestFixture : public ::testing::Test
{
public:
30 static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "test_is_battery_low" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
factory_->registerNodeType<nav2_behavior_tree::IsBatteryLowCondition>( "IsBatteryLow" );
battery_pub_ = node_->create_publisher<sensor_msgs::msg::BatteryState>(
"/battery_status",
rclcpp::SystemDefaultsQoS( ) );
}
51 static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
battery_pub_.reset( );
node_.reset( );
factory_.reset( );
}
protected:
61 static rclcpp::Node::SharedPtr node_;
62 static BT::NodeConfiguration * config_;
63 static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
64 static rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr battery_pub_;
};
rclcpp::Node::SharedPtr IsBatteryLowConditionTestFixture::node_ = nullptr;
BT::NodeConfiguration * IsBatteryLowConditionTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory> IsBatteryLowConditionTestFixture::factory_ = nullptr;
rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr
IsBatteryLowConditionTestFixture::battery_pub_ = nullptr;
73 TEST_F( IsBatteryLowConditionTestFixture, test_behavior_percentage )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<IsBatteryLow min_battery="0.5" battery_topic="/battery_status"/>
</BehaviorTree>
</root> )";
auto tree = factory_->createTreeFromText( xml_txt, config_->blackboard );
sensor_msgs::msg::BatteryState battery_msg;
battery_msg.percentage = 1.0;
battery_pub_->publish( battery_msg );
std::this_thread::sleep_for( std::chrono::milliseconds( 100 ) );
rclcpp::spin_some( node_ );
EXPECT_EQ( tree.tickRoot( ), BT::NodeStatus::FAILURE );
battery_msg.percentage = 0.49;
battery_pub_->publish( battery_msg );
std::this_thread::sleep_for( std::chrono::milliseconds( 100 ) );
rclcpp::spin_some( node_ );
EXPECT_EQ( tree.tickRoot( ), BT::NodeStatus::SUCCESS );
battery_msg.percentage = 0.51;
battery_pub_->publish( battery_msg );
std::this_thread::sleep_for( std::chrono::milliseconds( 100 ) );
rclcpp::spin_some( node_ );
EXPECT_EQ( tree.tickRoot( ), BT::NodeStatus::FAILURE );
battery_msg.percentage = 0.0;
battery_pub_->publish( battery_msg );
106 std::this_thread::sleep_for( std::chrono::milliseconds( 100 ) );
rclcpp::spin_some( node_ );
EXPECT_EQ( tree.tickRoot( ), BT::NodeStatus::SUCCESS );
}
TEST_F( IsBatteryLowConditionTestFixture, test_behavior_voltage )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<IsBatteryLow min_battery="5.0" battery_topic="/battery_status" is_voltage="true"/>
</BehaviorTree>
</root> )";
auto tree = factory_->createTreeFromText( xml_txt, config_->blackboard );
sensor_msgs::msg::BatteryState battery_msg;
battery_msg.voltage = 10.0;
battery_pub_->publish( battery_msg );
std::this_thread::sleep_for( std::chrono::milliseconds( 100 ) );
rclcpp::spin_some( node_ );
EXPECT_EQ( tree.tickRoot( ), BT::NodeStatus::FAILURE );
battery_msg.voltage = 4.9;
battery_pub_->publish( battery_msg );
std::this_thread::sleep_for( std::chrono::milliseconds( 100 ) );
rclcpp::spin_some( node_ );
EXPECT_EQ( tree.tickRoot( ), BT::NodeStatus::SUCCESS );
battery_msg.voltage = 5.1;
battery_pub_->publish( battery_msg );
std::this_thread::sleep_for( std::chrono::milliseconds( 100 ) );
139 rclcpp::spin_some( node_ );
EXPECT_EQ( tree.tickRoot( ), BT::NodeStatus::FAILURE );
battery_msg.voltage = 0.0;
battery_pub_->publish( battery_msg );
std::this_thread::sleep_for( std::chrono::milliseconds( 100 ) );
rclcpp::spin_some( node_ );
EXPECT_EQ( tree.tickRoot( ), BT::NodeStatus::SUCCESS );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2021 Joshua Wallace
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <chrono>
#include <memory>
#include <set>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/robot_utils.hpp"
#include "../../test_service.hpp"
#include "../../test_behavior_tree_fixture.hpp"
#include "nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp"
using namespace std::chrono; // NOLINT
using namespace std::chrono_literals; // NOLINT
32 class IsPathValidService : public TestService<nav2_msgs::srv::IsPathValid>
{
public:
35 IsPathValidService( )
: TestService( "is_path_valid" )
{}
39 virtual void handle_service(
40 const std::shared_ptr<rmw_request_id_t> request_header,
41 const std::shared_ptr<nav2_msgs::srv::IsPathValid::Request> request,
42 const std::shared_ptr<nav2_msgs::srv::IsPathValid::Response> response )
{
( void )request_header;
( void )request;
response->is_valid = true;
}
};
50 class IsPathValidTestFixture : public ::testing::Test
{
public:
53 void SetUp( )
{
node_ = std::make_shared<rclcpp::Node>( "test_is_path_valid_condition" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
config_->blackboard = BT::Blackboard::create( );
config_->blackboard->set<rclcpp::Node::SharedPtr>( "node", node_ );
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds( 10 ) );
factory_->registerNodeType<nav2_behavior_tree::IsPathValidCondition>( "IsPathValid" );
}
66 void TearDown( )
{
delete config_;
config_ = nullptr;
server_.reset( );
node_.reset( );
factory_.reset( );
tree_.reset( );
}
76 static std::shared_ptr<IsPathValidService> server_;
protected:
79 static rclcpp::Node::SharedPtr node_;
80 static BT::NodeConfiguration * config_;
81 static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
82 static std::shared_ptr<BT::Tree> tree_;
};
std::shared_ptr<IsPathValidService> IsPathValidTestFixture::server_ = nullptr;
rclcpp::Node::SharedPtr IsPathValidTestFixture::node_ = nullptr;
BT::NodeConfiguration * IsPathValidTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory> IsPathValidTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> IsPathValidTestFixture::tree_ = nullptr;
91 TEST_F( IsPathValidTestFixture, test_behavior )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<IsPathValid/>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
102 std::this_thread::sleep_for( 500ms );
EXPECT_EQ( tree_->rootNode( )->executeTick( ), BT::NodeStatus::SUCCESS );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
// initialize service and spin on new thread
IsPathValidTestFixture::server_ = std::make_shared<IsPathValidService>( );
std::thread server_thread( []( ) {
rclcpp::spin( IsPathValidTestFixture::server_ );
} );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
server_thread.join( );
return all_successful;
}
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <chrono>
#include <memory>
#include <set>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/robot_utils.hpp"
#include "../../test_behavior_tree_fixture.hpp"
#include "nav2_behavior_tree/plugins/condition/is_stuck_condition.hpp"
using namespace std::chrono; // NOLINT
using namespace std::chrono_literals; // NOLINT
31 class IsStuckTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
{
public:
34 void SetUp( )
{
bt_node_ = std::make_shared<nav2_behavior_tree::IsStuckCondition>( "is_stuck", *config_ );
}
39 void TearDown( )
{
bt_node_.reset( );
}
protected:
45 static std::shared_ptr<nav2_behavior_tree::IsStuckCondition> bt_node_;
};
std::shared_ptr<nav2_behavior_tree::IsStuckCondition>
IsStuckTestFixture::bt_node_ = nullptr;
51 TEST_F( IsStuckTestFixture, test_behavior )
{
auto odom_pub = node_->create_publisher<nav_msgs::msg::Odometry>( "odom", 1 );
nav_msgs::msg::Odometry odom_msg;
// fill up odometry history with zero velocity
auto time = node_->now( );
odom_msg.header.stamp = time;
odom_msg.twist.twist.linear.x = 0.0;
odom_pub->publish( odom_msg );
std::this_thread::sleep_for( 500ms );
odom_pub->publish( odom_msg );
std::this_thread::sleep_for( 500ms );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::FAILURE );
// huge negative velocity to simulate sudden brake
odom_msg.header.stamp = time + rclcpp::Duration::from_seconds( 0.1 );
odom_msg.twist.twist.linear.x = -1.5;
odom_pub->publish( odom_msg );
std::this_thread::sleep_for( 500ms );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
// huge positive velocity means robot is not stuck anymore
odom_msg.header.stamp = time + rclcpp::Duration::from_seconds( 0.2 );
odom_msg.twist.twist.linear.x = 1.0;
odom_pub->publish( odom_msg );
std::this_thread::sleep_for( 500ms );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::FAILURE );
// stuck again due to negative velocity change is smaller time period
odom_msg.header.stamp = time + rclcpp::Duration::from_seconds( 0.25 );
odom_msg.twist.twist.linear.x = 0.0;
odom_pub->publish( odom_msg );
std::this_thread::sleep_for( 500ms );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
}
88 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2022 Joshua Wallace
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <chrono>
#include <memory>
#include <set>
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/robot_utils.hpp"
#include "../../test_behavior_tree_fixture.hpp"
#include "nav2_behavior_tree/plugins/condition/path_expiring_timer_condition.hpp"
using namespace std::chrono; // NOLINT
using namespace std::chrono_literals; // NOLINT
29 class PathExpiringTimerConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
{
public:
32 void SetUp( )
{
node_ = std::make_shared<rclcpp::Node>( "test_path_expiring_condition" );
config_ = new BT::NodeConfiguration( );
config_->blackboard = BT::Blackboard::create( );
config_->blackboard->set<rclcpp::Node::SharedPtr>( "node", node_ );
bt_node_ = std::make_shared<nav2_behavior_tree::PathExpiringTimerCondition>(
"time_expired", *config_ );
}
42 void TearDown( )
{
delete config_;
config_ = nullptr;
node_.reset( );
bt_node_.reset( );
}
protected:
51 static rclcpp::Node::SharedPtr node_;
52 static std::shared_ptr<nav2_behavior_tree::PathExpiringTimerCondition> bt_node_;
53 static BT::NodeConfiguration * config_;
};
rclcpp::Node::SharedPtr PathExpiringTimerConditionTestFixture::node_ = nullptr;
std::shared_ptr<nav2_behavior_tree::PathExpiringTimerCondition>
PathExpiringTimerConditionTestFixture::bt_node_ = nullptr;
BT::NodeConfiguration * PathExpiringTimerConditionTestFixture::config_ = nullptr;
61 TEST_F( PathExpiringTimerConditionTestFixture, test_behavior )
{
EXPECT_EQ( bt_node_->status( ), BT::NodeStatus::IDLE );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::FAILURE );
for ( int i = 0; i < 20; ++i ) {
rclcpp::sleep_for( 500ms );
if ( i % 2 ) {
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
} else {
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::FAILURE );
}
}
// place a new path on the blackboard to reset the timer
nav_msgs::msg::Path path;
geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x = 1.0;
path.poses.push_back( pose );
config_->blackboard->set<nav_msgs::msg::Path>( "path", path );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::FAILURE );
rclcpp::sleep_for( 1500ms );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
}
87 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <chrono>
#include <memory>
#include <set>
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/robot_utils.hpp"
#include "../../test_behavior_tree_fixture.hpp"
#include "nav2_behavior_tree/plugins/condition/time_expired_condition.hpp"
using namespace std::chrono; // NOLINT
using namespace std::chrono_literals; // NOLINT
30 class TimeExpiredConditionTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
{
public:
33 void SetUp( )
{
bt_node_ = std::make_shared<nav2_behavior_tree::TimeExpiredCondition>(
"time_expired", *config_ );
}
39 void TearDown( )
{
bt_node_.reset( );
}
protected:
45 static std::shared_ptr<nav2_behavior_tree::TimeExpiredCondition> bt_node_;
};
std::shared_ptr<nav2_behavior_tree::TimeExpiredCondition>
TimeExpiredConditionTestFixture::bt_node_ = nullptr;
51 TEST_F( TimeExpiredConditionTestFixture, test_behavior )
{
EXPECT_EQ( bt_node_->status( ), BT::NodeStatus::IDLE );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::FAILURE );
for ( int i = 0; i < 20; ++i ) {
rclcpp::sleep_for( 500ms );
if ( i % 2 ) {
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
} else {
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::FAILURE );
}
}
}
66 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <chrono>
#include <string>
#include "../../test_behavior_tree_fixture.hpp"
#include "nav2_behavior_tree/plugins/condition/transform_available_condition.hpp"
25 class TransformAvailableConditionTestFixture : public ::testing::Test
{
public:
28 static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "test_behavior_tree_fixture" );
transform_handler_ = std::make_shared<nav2_behavior_tree::TransformHandler>( node_ );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
config_->blackboard->set<std::shared_ptr<tf2_ros::Buffer>>(
"tf_buffer",
transform_handler_->getBuffer( ) );
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds( 20 ) );
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds( 10 ) );
config_->blackboard->set<bool>( "initial_pose_received", false );
}
54 static void TearDownTestCase( )
{
transform_handler_->deactivate( );
delete config_;
config_ = nullptr;
transform_handler_.reset( );
node_.reset( );
factory_.reset( );
}
64 void SetUp( )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<TransformAvailable child="base_link" parent="map" />
</BehaviorTree>
</root> )";
74 factory_->registerNodeType<nav2_behavior_tree::TransformAvailableCondition>(
"TransformAvailable" );
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
}
void TearDown( )
80 {
81 tree_.reset( );
82 }
83
84 protected:
static rclcpp::Node::SharedPtr node_;
static std::shared_ptr<nav2_behavior_tree::TransformHandler> transform_handler_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr TransformAvailableConditionTestFixture::node_ = nullptr;
std::shared_ptr<nav2_behavior_tree::TransformHandler>
TransformAvailableConditionTestFixture::transform_handler_ = nullptr;
95 BT::NodeConfiguration * TransformAvailableConditionTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory>
TransformAvailableConditionTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> TransformAvailableConditionTestFixture::tree_ = nullptr;
TEST_F( TransformAvailableConditionTestFixture, test_behavior )
{
EXPECT_EQ( tree_->tickRoot( ), BT::NodeStatus::FAILURE );
103 transform_handler_->activate( );
transform_handler_->waitForTransform( );
EXPECT_EQ( tree_->tickRoot( ), BT::NodeStatus::SUCCESS );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include "../../test_behavior_tree_fixture.hpp"
#include "../../test_dummy_tree_node.hpp"
#include "nav2_behavior_tree/plugins/control/pipeline_sequence.hpp"
23 class PipelineSequenceTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
{
public:
void SetUp( ) override
{
bt_node_ = std::make_shared<nav2_behavior_tree::PipelineSequence>(
"pipeline_sequence", *config_ );
first_child_ = std::make_shared<nav2_behavior_tree::DummyNode>( );
second_child_ = std::make_shared<nav2_behavior_tree::DummyNode>( );
third_child_ = std::make_shared<nav2_behavior_tree::DummyNode>( );
bt_node_->addChild( first_child_.get( ) );
bt_node_->addChild( second_child_.get( ) );
bt_node_->addChild( third_child_.get( ) );
}
void TearDown( ) override
{
first_child_.reset( );
41 second_child_.reset( );
42 third_child_.reset( );
bt_node_.reset( );
}
protected:
static std::shared_ptr<nav2_behavior_tree::PipelineSequence> bt_node_;
static std::shared_ptr<nav2_behavior_tree::DummyNode> first_child_;
static std::shared_ptr<nav2_behavior_tree::DummyNode> second_child_;
static std::shared_ptr<nav2_behavior_tree::DummyNode> third_child_;
};
std::shared_ptr<nav2_behavior_tree::PipelineSequence>
PipelineSequenceTestFixture::bt_node_ = nullptr;
std::shared_ptr<nav2_behavior_tree::DummyNode>
PipelineSequenceTestFixture::first_child_ = nullptr;
std::shared_ptr<nav2_behavior_tree::DummyNode>
PipelineSequenceTestFixture::second_child_ = nullptr;
std::shared_ptr<nav2_behavior_tree::DummyNode>
PipelineSequenceTestFixture::third_child_ = nullptr;
TEST_F( PipelineSequenceTestFixture, test_failure_on_idle_child )
{
first_child_->changeStatus( BT::NodeStatus::IDLE );
EXPECT_THROW( bt_node_->executeTick( ), std::runtime_error );
}
TEST_F( PipelineSequenceTestFixture, test_failure )
{
first_child_->changeStatus( BT::NodeStatus::FAILURE );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::FAILURE );
EXPECT_EQ( first_child_->status( ), BT::NodeStatus::IDLE );
EXPECT_EQ( second_child_->status( ), BT::NodeStatus::IDLE );
EXPECT_EQ( third_child_->status( ), BT::NodeStatus::IDLE );
first_child_->changeStatus( BT::NodeStatus::SUCCESS );
second_child_->changeStatus( BT::NodeStatus::FAILURE );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::FAILURE );
EXPECT_EQ( first_child_->status( ), BT::NodeStatus::IDLE );
EXPECT_EQ( second_child_->status( ), BT::NodeStatus::IDLE );
EXPECT_EQ( third_child_->status( ), BT::NodeStatus::IDLE );
first_child_->changeStatus( BT::NodeStatus::SUCCESS );
second_child_->changeStatus( BT::NodeStatus::SUCCESS );
third_child_->changeStatus( BT::NodeStatus::FAILURE );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::FAILURE );
EXPECT_EQ( first_child_->status( ), BT::NodeStatus::IDLE );
EXPECT_EQ( second_child_->status( ), BT::NodeStatus::IDLE );
EXPECT_EQ( third_child_->status( ), BT::NodeStatus::IDLE );
first_child_->changeStatus( BT::NodeStatus::SUCCESS );
second_child_->changeStatus( BT::NodeStatus::SUCCESS );
third_child_->changeStatus( BT::NodeStatus::RUNNING );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::RUNNING );
first_child_->changeStatus( BT::NodeStatus::FAILURE );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::FAILURE );
EXPECT_EQ( first_child_->status( ), BT::NodeStatus::IDLE );
EXPECT_EQ( second_child_->status( ), BT::NodeStatus::IDLE );
EXPECT_EQ( third_child_->status( ), BT::NodeStatus::IDLE );
}
TEST_F( PipelineSequenceTestFixture, test_behavior )
{
first_child_->changeStatus( BT::NodeStatus::RUNNING );
second_child_->changeStatus( BT::NodeStatus::IDLE );
third_child_->changeStatus( BT::NodeStatus::IDLE );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::RUNNING );
first_child_->changeStatus( BT::NodeStatus::SUCCESS );
second_child_->changeStatus( BT::NodeStatus::RUNNING );
third_child_->changeStatus( BT::NodeStatus::IDLE );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::RUNNING );
first_child_->changeStatus( BT::NodeStatus::RUNNING );
second_child_->changeStatus( BT::NodeStatus::SUCCESS );
third_child_->changeStatus( BT::NodeStatus::RUNNING );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::RUNNING );
first_child_->changeStatus( BT::NodeStatus::RUNNING );
second_child_->changeStatus( BT::NodeStatus::SUCCESS );
third_child_->changeStatus( BT::NodeStatus::SUCCESS );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( first_child_->status( ), BT::NodeStatus::IDLE );
EXPECT_EQ( second_child_->status( ), BT::NodeStatus::IDLE );
EXPECT_EQ( third_child_->status( ), BT::NodeStatus::IDLE );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include "../../test_behavior_tree_fixture.hpp"
#include "../../test_dummy_tree_node.hpp"
#include "nav2_behavior_tree/plugins/control/recovery_node.hpp"
// Changes status to SUCCESS after a specified number of failures
24 class RecoveryDummy : public nav2_behavior_tree::DummyNode
{
public:
BT::NodeStatus tick( ) override
{
if ( ticks_ == num_success_ ) {
setStatus( BT::NodeStatus::SUCCESS );
} else if ( ticks_ == num_failure_ ) {
setStatus( BT::NodeStatus::FAILURE );
}
ticks_++;
return status( );
}
void returnSuccessOn( int tick )
{
num_success_ = tick;
ticks_ = 0;
}
void returnFailureOn( int tick )
{
num_failure_ = tick;
ticks_ = 0;
}
private:
int ticks_{0};
int num_success_{-1};
int num_failure_{-1};
};
class RecoveryNodeTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
{
public:
void SetUp( ) override
{
bt_node_ = std::make_shared<nav2_behavior_tree::RecoveryNode>(
"recovery_node", *config_ );
first_child_ = std::make_shared<RecoveryDummy>( );
second_child_ = std::make_shared<RecoveryDummy>( );
bt_node_->addChild( first_child_.get( ) );
bt_node_->addChild( second_child_.get( ) );
}
void TearDown( ) override
{
first_child_.reset( );
second_child_.reset( );
bt_node_.reset( );
}
protected:
static std::shared_ptr<nav2_behavior_tree::RecoveryNode> bt_node_;
static std::shared_ptr<RecoveryDummy> first_child_;
static std::shared_ptr<RecoveryDummy> second_child_;
};
std::shared_ptr<nav2_behavior_tree::RecoveryNode> RecoveryNodeTestFixture::bt_node_ = nullptr;
std::shared_ptr<RecoveryDummy> RecoveryNodeTestFixture::first_child_ = nullptr;
std::shared_ptr<RecoveryDummy> RecoveryNodeTestFixture::second_child_ = nullptr;
TEST_F( RecoveryNodeTestFixture, test_only_two_children )
{
nav2_behavior_tree::DummyNode dummy;
bt_node_->addChild( &dummy );
EXPECT_THROW( bt_node_->executeTick( ), BT::BehaviorTreeException );
}
TEST_F( RecoveryNodeTestFixture, test_running )
{
first_child_->changeStatus( BT::NodeStatus::RUNNING );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::RUNNING );
first_child_->changeStatus( BT::NodeStatus::FAILURE );
second_child_->changeStatus( BT::NodeStatus::RUNNING );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::RUNNING );
}
TEST_F( RecoveryNodeTestFixture, test_failure_on_idle_child )
{
first_child_->changeStatus( BT::NodeStatus::IDLE );
EXPECT_THROW( bt_node_->executeTick( ), BT::LogicError );
first_child_->changeStatus( BT::NodeStatus::FAILURE );
second_child_->changeStatus( BT::NodeStatus::IDLE );
EXPECT_THROW( bt_node_->executeTick( ), BT::LogicError );
}
TEST_F( RecoveryNodeTestFixture, test_success_one_retry )
{
// first child returns success right away
first_child_->changeStatus( BT::NodeStatus::SUCCESS );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
// first child fails, second child succeeds, then first child succeeds ( one retry )
first_child_->returnSuccessOn( 1 );
first_child_->changeStatus( BT::NodeStatus::FAILURE );
second_child_->changeStatus( BT::NodeStatus::SUCCESS );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( bt_node_->status( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( first_child_->status( ), BT::NodeStatus::IDLE );
EXPECT_EQ( second_child_->status( ), BT::NodeStatus::IDLE );
}
TEST_F( RecoveryNodeTestFixture, test_failure_one_retry )
{
// first child fails, second child fails
first_child_->changeStatus( BT::NodeStatus::FAILURE );
second_child_->changeStatus( BT::NodeStatus::FAILURE );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::FAILURE );
EXPECT_EQ( bt_node_->status( ), BT::NodeStatus::FAILURE );
EXPECT_EQ( first_child_->status( ), BT::NodeStatus::IDLE );
EXPECT_EQ( second_child_->status( ), BT::NodeStatus::IDLE );
// first child fails, second child succeeds, then first child fails ( one retry )
first_child_->returnFailureOn( 1 );
first_child_->changeStatus( BT::NodeStatus::FAILURE );
second_child_->changeStatus( BT::NodeStatus::SUCCESS );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::FAILURE );
EXPECT_EQ( bt_node_->status( ), BT::NodeStatus::FAILURE );
EXPECT_EQ( first_child_->status( ), BT::NodeStatus::IDLE );
EXPECT_EQ( second_child_->status( ), BT::NodeStatus::IDLE );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include "../../test_behavior_tree_fixture.hpp"
#include "../../test_dummy_tree_node.hpp"
#include "nav2_behavior_tree/plugins/control/round_robin_node.hpp"
23 class RoundRobinNodeTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
{
public:
void SetUp( ) override
{
bt_node_ = std::make_shared<nav2_behavior_tree::RoundRobinNode>(
"round_robin", *config_ );
first_child_ = std::make_shared<nav2_behavior_tree::DummyNode>( );
second_child_ = std::make_shared<nav2_behavior_tree::DummyNode>( );
third_child_ = std::make_shared<nav2_behavior_tree::DummyNode>( );
bt_node_->addChild( first_child_.get( ) );
bt_node_->addChild( second_child_.get( ) );
bt_node_->addChild( third_child_.get( ) );
}
void TearDown( ) override
{
first_child_.reset( );
41 second_child_.reset( );
42 third_child_.reset( );
bt_node_.reset( );
}
protected:
static std::shared_ptr<nav2_behavior_tree::RoundRobinNode> bt_node_;
static std::shared_ptr<nav2_behavior_tree::DummyNode> first_child_;
static std::shared_ptr<nav2_behavior_tree::DummyNode> second_child_;
static std::shared_ptr<nav2_behavior_tree::DummyNode> third_child_;
};
std::shared_ptr<nav2_behavior_tree::RoundRobinNode> RoundRobinNodeTestFixture::bt_node_ = nullptr;
std::shared_ptr<nav2_behavior_tree::DummyNode> RoundRobinNodeTestFixture::first_child_ = nullptr;
std::shared_ptr<nav2_behavior_tree::DummyNode> RoundRobinNodeTestFixture::second_child_ = nullptr;
std::shared_ptr<nav2_behavior_tree::DummyNode> RoundRobinNodeTestFixture::third_child_ = nullptr;
TEST_F( RoundRobinNodeTestFixture, test_failure_on_idle_child )
{
first_child_->changeStatus( BT::NodeStatus::IDLE );
EXPECT_THROW( bt_node_->executeTick( ), BT::LogicError );
}
TEST_F( RoundRobinNodeTestFixture, test_failure )
{
first_child_->changeStatus( BT::NodeStatus::FAILURE );
second_child_->changeStatus( BT::NodeStatus::RUNNING );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::RUNNING );
second_child_->changeStatus( BT::NodeStatus::FAILURE );
third_child_->changeStatus( BT::NodeStatus::RUNNING );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::RUNNING );
third_child_->changeStatus( BT::NodeStatus::FAILURE );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::FAILURE );
EXPECT_EQ( first_child_->status( ), BT::NodeStatus::IDLE );
EXPECT_EQ( second_child_->status( ), BT::NodeStatus::IDLE );
EXPECT_EQ( third_child_->status( ), BT::NodeStatus::IDLE );
}
TEST_F( RoundRobinNodeTestFixture, test_behavior )
{
first_child_->changeStatus( BT::NodeStatus::RUNNING );
second_child_->changeStatus( BT::NodeStatus::IDLE );
third_child_->changeStatus( BT::NodeStatus::IDLE );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::RUNNING );
first_child_->changeStatus( BT::NodeStatus::FAILURE );
second_child_->changeStatus( BT::NodeStatus::RUNNING );
third_child_->changeStatus( BT::NodeStatus::IDLE );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::RUNNING );
first_child_->changeStatus( BT::NodeStatus::FAILURE );
second_child_->changeStatus( BT::NodeStatus::SUCCESS );
third_child_->changeStatus( BT::NodeStatus::IDLE );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
first_child_->changeStatus( BT::NodeStatus::IDLE );
second_child_->changeStatus( BT::NodeStatus::IDLE );
third_child_->changeStatus( BT::NodeStatus::RUNNING );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::RUNNING );
first_child_->changeStatus( BT::NodeStatus::RUNNING );
second_child_->changeStatus( BT::NodeStatus::IDLE );
third_child_->changeStatus( BT::NodeStatus::FAILURE );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::RUNNING );
first_child_->changeStatus( BT::NodeStatus::FAILURE );
second_child_->changeStatus( BT::NodeStatus::SUCCESS );
third_child_->changeStatus( BT::NodeStatus::FAILURE );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( first_child_->status( ), BT::NodeStatus::IDLE );
EXPECT_EQ( second_child_->status( ), BT::NodeStatus::IDLE );
EXPECT_EQ( third_child_->status( ), BT::NodeStatus::IDLE );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <cmath>
#include <memory>
#include <set>
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/robot_utils.hpp"
#include "../../test_behavior_tree_fixture.hpp"
#include "../../test_dummy_tree_node.hpp"
#include "nav2_behavior_tree/plugins/decorator/distance_controller.hpp"
28 class DistanceControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
{
public:
31 void SetUp( )
{
bt_node_ = std::make_shared<nav2_behavior_tree::DistanceController>(
"distance_controller", *config_ );
dummy_node_ = std::make_shared<nav2_behavior_tree::DummyNode>( );
bt_node_->setChild( dummy_node_.get( ) );
}
39 void TearDown( )
{
dummy_node_.reset( );
bt_node_.reset( );
}
protected:
46 static std::shared_ptr<nav2_behavior_tree::DistanceController> bt_node_;
47 static std::shared_ptr<nav2_behavior_tree::DummyNode> dummy_node_;
};
std::shared_ptr<nav2_behavior_tree::DistanceController>
DistanceControllerTestFixture::bt_node_ = nullptr;
std::shared_ptr<nav2_behavior_tree::DummyNode>
DistanceControllerTestFixture::dummy_node_ = nullptr;
55 TEST_F( DistanceControllerTestFixture, test_behavior )
{
EXPECT_EQ( bt_node_->status( ), BT::NodeStatus::IDLE );
dummy_node_->changeStatus( BT::NodeStatus::SUCCESS );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( dummy_node_->status( ), BT::NodeStatus::IDLE );
geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.orientation.w = 1;
double traveled = 0;
for ( int i = 1; i <= 20; i++ ) {
pose.pose.position.x = i * 0.51;
transform_handler_->updateRobotPose( pose.pose );
// Wait for transforms to actually update
// updated pose is i * 0.55
// we wait fot the traveled distance to reach a value > i * 0.5
// we can assume the current transform has been updated at this point
while ( traveled < i * 0.5 ) {
if ( nav2_util::getCurrentPose( pose, *transform_handler_->getBuffer( ) ) ) {
traveled = std::sqrt(
pose.pose.position.x * pose.pose.position.x +
pose.pose.position.y * pose.pose.position.y );
}
}
dummy_node_->changeStatus( BT::NodeStatus::SUCCESS );
if ( i % 2 ) {
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::RUNNING );
} else {
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( dummy_node_->status( ), BT::NodeStatus::IDLE );
}
}
}
96 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <chrono>
#include <memory>
#include <set>
#include "../../test_behavior_tree_fixture.hpp"
#include "nav2_behavior_tree/plugins/decorator/goal_updated_controller.hpp"
using namespace std::chrono; // NOLINT
using namespace std::chrono_literals; // NOLINT
27 class GoalUpdatedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
{
public:
30 void SetUp( )
{
// Setting fake goals on blackboard
geometry_msgs::msg::PoseStamped goal1;
goal1.header.stamp = node_->now( );
std::vector<geometry_msgs::msg::PoseStamped> poses1;
poses1.push_back( goal1 );
config_->blackboard->set( "goal", goal1 );
config_->blackboard->set<std::vector<geometry_msgs::msg::PoseStamped>>( "goals", poses1 );
bt_node_ = std::make_shared<nav2_behavior_tree::GoalUpdatedController>(
"goal_updated_controller", *config_ );
dummy_node_ = std::make_shared<nav2_behavior_tree::DummyNode>( );
bt_node_->setChild( dummy_node_.get( ) );
}
46 void TearDown( )
{
dummy_node_.reset( );
bt_node_.reset( );
}
protected:
53 static std::shared_ptr<nav2_behavior_tree::GoalUpdatedController> bt_node_;
54 static std::shared_ptr<nav2_behavior_tree::DummyNode> dummy_node_;
};
std::shared_ptr<nav2_behavior_tree::GoalUpdatedController>
GoalUpdatedControllerTestFixture::bt_node_ = nullptr;
std::shared_ptr<nav2_behavior_tree::DummyNode>
GoalUpdatedControllerTestFixture::dummy_node_ = nullptr;
62 TEST_F( GoalUpdatedControllerTestFixture, test_behavior )
{
// Creating updated fake-goals
geometry_msgs::msg::PoseStamped goal2;
goal2.header.stamp = node_->now( );
std::vector<geometry_msgs::msg::PoseStamped> poses2;
poses2.push_back( goal2 );
// starting in idle
EXPECT_EQ( bt_node_->status( ), BT::NodeStatus::IDLE );
// tick for the first time, dummy node should be ticked
dummy_node_->changeStatus( BT::NodeStatus::SUCCESS );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( dummy_node_->status( ), BT::NodeStatus::IDLE );
// tick again with updated goal, dummy node should be ticked
config_->blackboard->set( "goal", goal2 );
dummy_node_->changeStatus( BT::NodeStatus::SUCCESS );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( dummy_node_->status( ), BT::NodeStatus::IDLE );
// tick again without update, dummy node should not be ticked
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::RUNNING );
EXPECT_EQ( dummy_node_->status( ), BT::NodeStatus::IDLE );
// tick again with updated goals, dummy node should be ticked
config_->blackboard->set<std::vector<geometry_msgs::msg::PoseStamped>>( "goals", poses2 );
dummy_node_->changeStatus( BT::NodeStatus::SUCCESS );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( dummy_node_->status( ), BT::NodeStatus::IDLE );
}
95 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Francisco Martin Rico
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include <string>
#include "nav_msgs/msg/path.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "../../test_action_server.hpp"
#include "nav2_behavior_tree/plugins/decorator/goal_updater_node.hpp"
30 class GoalUpdaterTestFixture : public ::testing::Test
{
public:
33 static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "goal_updater_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::GoalUpdater>(
name, config );
};
factory_->registerBuilder<nav2_behavior_tree::GoalUpdater>(
"GoalUpdater", builder );
}
58 static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
factory_.reset( );
}
void TearDown( ) override
{
tree_.reset( );
}
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr GoalUpdaterTestFixture::node_ = nullptr;
BT::NodeConfiguration * GoalUpdaterTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory> GoalUpdaterTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> GoalUpdaterTestFixture::tree_ = nullptr;
TEST_F( GoalUpdaterTestFixture, test_tick )
{
// create tree
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
<AlwaysSuccess/>
</GoalUpdater>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
// create new goal and set it on blackboard
geometry_msgs::msg::PoseStamped goal;
goal.header.stamp = node_->now( );
goal.pose.position.x = 1.0;
config_->blackboard->set( "goal", goal );
// tick until node succeeds
while ( tree_->rootNode( )->status( ) != BT::NodeStatus::SUCCESS ) {
tree_->rootNode( )->executeTick( );
}
geometry_msgs::msg::PoseStamped updated_goal;
config_->blackboard->get( "updated_goal", updated_goal );
EXPECT_EQ( updated_goal, goal );
geometry_msgs::msg::PoseStamped goal_to_update;
goal_to_update.header.stamp = node_->now( );
goal_to_update.pose.position.x = 2.0;
auto goal_updater_pub =
node_->create_publisher<geometry_msgs::msg::PoseStamped>( "goal_update", 10 );
auto start = node_->now( );
while ( ( node_->now( ) - start ).seconds( ) < 0.5 ) {
tree_->rootNode( )->executeTick( );
goal_updater_pub->publish( goal_to_update );
rclcpp::spin_some( node_ );
}
config_->blackboard->get( "updated_goal", updated_goal );
EXPECT_NE( updated_goal, goal );
EXPECT_EQ( updated_goal, goal_to_update );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
// Copyright ( c ) 2022 Neobotix GmbH
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <chrono>
#include <memory>
#include <set>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav_msgs/msg/path.hpp"
#include "../../test_behavior_tree_fixture.hpp"
#include "../../test_dummy_tree_node.hpp"
#include "nav2_behavior_tree/plugins/decorator/path_longer_on_approach.hpp"
using namespace std::chrono; // NOLINT
using namespace std::chrono_literals; // NOLINT
32 class PathLongerOnApproachTestFixture : public ::testing::Test
{
public:
35 static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "path_longer_on_approach_test_fixture" );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
BT::NodeBuilder builder =
[]( const std::string & name, const BT::NodeConfiguration & config )
{
return std::make_unique<nav2_behavior_tree::PathLongerOnApproach>(
name, config );
};
factory_->registerBuilder<nav2_behavior_tree::PathLongerOnApproach>(
"PathLongerOnApproach", builder );
}
60 static void TearDownTestCase( )
{
delete config_;
config_ = nullptr;
node_.reset( );
factory_.reset( );
}
void TearDown( ) override
{
tree_.reset( );
}
protected:
static rclcpp::Node::SharedPtr node_;
static BT::NodeConfiguration * config_;
static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
static std::shared_ptr<BT::Tree> tree_;
};
rclcpp::Node::SharedPtr PathLongerOnApproachTestFixture::node_ = nullptr;
BT::NodeConfiguration * PathLongerOnApproachTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory> PathLongerOnApproachTestFixture::factory_ = nullptr;
std::shared_ptr<BT::Tree> PathLongerOnApproachTestFixture::tree_ = nullptr;
TEST_F( PathLongerOnApproachTestFixture, test_tick )
{
// Success test
// create tree
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<PathLongerOnApproach path="{path}" prox_len="5.0" length_factor="2.0">
<AlwaysSuccess/>
</PathLongerOnApproach>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
// set new path on blackboard
nav_msgs::msg::Path new_path;
new_path.poses.resize( 10 );
for ( unsigned int i = 0; i < new_path.poses.size( ); i++ ) {
// Assuming distance between waypoints to be 1.5m
new_path.poses[i].pose.position.x = 1.5 * i;
}
config_->blackboard->set<nav_msgs::msg::Path>( "path", new_path );
tree_->rootNode( )->executeTick( );
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::SUCCESS );
// Failure test
// create tree
xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<PathLongerOnApproach path="{path}" prox_len="20.0" length_factor="1.0">
<AlwaysFailure/>
</PathLongerOnApproach>
</BehaviorTree>
</root> )";
tree_ = std::make_shared<BT::Tree>( factory_->createTreeFromText( xml_txt, config_->blackboard ) );
// set old path on blackboard
nav_msgs::msg::Path old_path;
old_path.poses.resize( 5 );
for ( unsigned int i = 1; i <= old_path.poses.size( ); i++ ) {
// Assuming distance between waypoints to be 3.0m
old_path.poses[i - 1].pose.position.x = 3.0 * i;
}
config_->blackboard->set<nav_msgs::msg::Path>( "path", old_path );
tree_->rootNode( )->executeTick( );
// set new path on blackboard
new_path.poses.resize( 11 );
for ( unsigned int i = 0; i <= new_path.poses.size( ); i++ ) {
// Assuming distance between waypoints to be 1.5m
new_path.poses[i].pose.position.x = 1.5 * i;
}
config_->blackboard->set<nav_msgs::msg::Path>( "path", new_path );
tree_->rootNode( )->executeTick( );
EXPECT_EQ( tree_->rootNode( )->status( ), BT::NodeStatus::FAILURE );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
int all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <chrono>
#include <memory>
#include <set>
#include "../../test_behavior_tree_fixture.hpp"
#include "nav2_behavior_tree/plugins/decorator/rate_controller.hpp"
using namespace std::chrono; // NOLINT
using namespace std::chrono_literals; // NOLINT
27 class RateControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
{
public:
30 void SetUp( )
{
bt_node_ = std::make_shared<nav2_behavior_tree::RateController>(
"rate_controller", *config_ );
dummy_node_ = std::make_shared<nav2_behavior_tree::DummyNode>( );
bt_node_->setChild( dummy_node_.get( ) );
}
38 void TearDown( )
{
dummy_node_.reset( );
bt_node_.reset( );
}
protected:
45 static std::shared_ptr<nav2_behavior_tree::RateController> bt_node_;
46 static std::shared_ptr<nav2_behavior_tree::DummyNode> dummy_node_;
};
std::shared_ptr<nav2_behavior_tree::RateController>
RateControllerTestFixture::bt_node_ = nullptr;
std::shared_ptr<nav2_behavior_tree::DummyNode>
RateControllerTestFixture::dummy_node_ = nullptr;
54 TEST_F( RateControllerTestFixture, test_behavior )
{
EXPECT_EQ( bt_node_->status( ), BT::NodeStatus::IDLE );
dummy_node_->changeStatus( BT::NodeStatus::SUCCESS );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( dummy_node_->status( ), BT::NodeStatus::IDLE );
for ( int i = 0; i < 10; ++i ) {
dummy_node_->changeStatus( BT::NodeStatus::SUCCESS );
std::this_thread::sleep_for( 500ms );
if ( i % 2 ) {
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( dummy_node_->status( ), BT::NodeStatus::IDLE );
} else {
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::RUNNING );
}
}
}
74 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <chrono>
#include <memory>
#include <set>
#include "../../test_behavior_tree_fixture.hpp"
#include "nav2_behavior_tree/plugins/decorator/single_trigger_node.hpp"
using namespace std::chrono; // NOLINT
using namespace std::chrono_literals; // NOLINT
27 class SingleTriggerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
{
public:
30 void SetUp( )
{
bt_node_ = std::make_shared<nav2_behavior_tree::SingleTrigger>(
"single_trigger", *config_ );
dummy_node_ = std::make_shared<nav2_behavior_tree::DummyNode>( );
bt_node_->setChild( dummy_node_.get( ) );
}
38 void TearDown( )
{
dummy_node_.reset( );
bt_node_.reset( );
}
protected:
45 static std::shared_ptr<nav2_behavior_tree::SingleTrigger> bt_node_;
46 static std::shared_ptr<nav2_behavior_tree::DummyNode> dummy_node_;
};
std::shared_ptr<nav2_behavior_tree::SingleTrigger>
SingleTriggerTestFixture::bt_node_ = nullptr;
std::shared_ptr<nav2_behavior_tree::DummyNode>
SingleTriggerTestFixture::dummy_node_ = nullptr;
54 TEST_F( SingleTriggerTestFixture, test_behavior )
{
// starting in idle
EXPECT_EQ( bt_node_->status( ), BT::NodeStatus::IDLE );
// tick once, should work
dummy_node_->changeStatus( BT::NodeStatus::SUCCESS );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( dummy_node_->status( ), BT::NodeStatus::IDLE );
// tick again with dummy node success, should fail
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::FAILURE );
// tick again with dummy node idle, should still fail
dummy_node_->changeStatus( BT::NodeStatus::IDLE );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::FAILURE );
// halt BT for a new execution run, should work when dummy node is running
// and once when dummy node returns success and then fail
bt_node_->halt( );
dummy_node_->changeStatus( BT::NodeStatus::RUNNING );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::RUNNING );
dummy_node_->changeStatus( BT::NodeStatus::SUCCESS );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::FAILURE );
}
81 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <chrono>
#include <memory>
#include <set>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/robot_utils.hpp"
#include "../../test_behavior_tree_fixture.hpp"
#include "../../test_dummy_tree_node.hpp"
#include "nav2_behavior_tree/plugins/decorator/speed_controller.hpp"
using namespace std::chrono; // NOLINT
using namespace std::chrono_literals; // NOLINT
32 class SpeedControllerTestFixture : public nav2_behavior_tree::BehaviorTreeTestFixture
{
public:
35 void SetUp( )
{
geometry_msgs::msg::PoseStamped goal;
goal.header.stamp = node_->now( );
config_->blackboard->set( "goal", goal );
std::vector<geometry_msgs::msg::PoseStamped> fake_poses;
config_->blackboard->set<std::vector<geometry_msgs::msg::PoseStamped>>( "goals", fake_poses ); // NOLINT
bt_node_ = std::make_shared<nav2_behavior_tree::SpeedController>( "speed_controller", *config_ );
dummy_node_ = std::make_shared<nav2_behavior_tree::DummyNode>( );
bt_node_->setChild( dummy_node_.get( ) );
}
49 void TearDown( )
{
dummy_node_.reset( );
bt_node_.reset( );
}
protected:
56 static std::shared_ptr<nav2_behavior_tree::SpeedController> bt_node_;
57 static std::shared_ptr<nav2_behavior_tree::DummyNode> dummy_node_;
};
std::shared_ptr<nav2_behavior_tree::SpeedController>
SpeedControllerTestFixture::bt_node_ = nullptr;
std::shared_ptr<nav2_behavior_tree::DummyNode>
SpeedControllerTestFixture::dummy_node_ = nullptr;
/*
* Test for speed controller behavior
* Speed controller calculates the period after which it should succeed
* based on the current velocity which is scaled to a pre-defined rate range
* Current velocity is set using odom messages
* The period is reset on the basis of current velocity after the last period is exceeded
*/
72 TEST_F( SpeedControllerTestFixture, test_behavior )
{
auto odom_pub = node_->create_publisher<nav_msgs::msg::Odometry>( "odom", 1 );
nav_msgs::msg::Odometry odom_msg;
auto time = node_->now( );
odom_msg.header.stamp = time;
odom_msg.twist.twist.linear.x = 0.223;
odom_pub->publish( odom_msg );
EXPECT_EQ( bt_node_->status( ), BT::NodeStatus::IDLE );
dummy_node_->changeStatus( BT::NodeStatus::SUCCESS );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
EXPECT_EQ( dummy_node_->status( ), BT::NodeStatus::IDLE );
// after the first tick, period should be a default value of 1s
// first tick should return running since period has not exceeded
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::RUNNING );
// set the child node to success so node can return success
dummy_node_->changeStatus( BT::NodeStatus::SUCCESS );
// should return success since period has exceeded and new period should be set to ~2s
rclcpp::sleep_for( 1s );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
// send new velocity for update after the next period
odom_msg.header.stamp = time + rclcpp::Duration::from_seconds( 0.5 );
odom_msg.twist.twist.linear.x = 0;
odom_msg.twist.twist.linear.y = 0;
odom_pub->publish( odom_msg );
// Period should be set to ~2s based on the last speed of 0.223 m/s
rclcpp::sleep_for( 1s );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::RUNNING );
dummy_node_->changeStatus( BT::NodeStatus::SUCCESS );
rclcpp::sleep_for( 1s );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
// period should be set to ~10s based on the last speed of 0 m/s
// should return running for the first 9 seconds
for ( int i = 0; i < 9; ++i ) {
rclcpp::sleep_for( 1s );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::RUNNING );
}
// set the child node to success so node can return success
dummy_node_->changeStatus( BT::NodeStatus::SUCCESS );
// should return success since period has exceeded
rclcpp::sleep_for( 1s );
EXPECT_EQ( bt_node_->executeTick( ), BT::NodeStatus::SUCCESS );
}
128 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TEST_ACTION_SERVER_HPP_
#define TEST_ACTION_SERVER_HPP_
#include <string>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
template<class ActionT>
25 class TestActionServer : public rclcpp::Node
{
public:
28 explicit TestActionServer(
std::string action_name,
const rclcpp::NodeOptions & options = rclcpp::NodeOptions( ) )
: Node( "test_action_server", options )
{
using namespace std::placeholders; // NOLINT
this->action_server_ = rclcpp_action::create_server<ActionT>(
this->get_node_base_interface( ),
this->get_node_clock_interface( ),
this->get_node_logging_interface( ),
this->get_node_waitables_interface( ),
action_name,
std::bind( &TestActionServer::handle_goal, this, _1, _2 ),
std::bind( &TestActionServer::handle_cancel, this, _1 ),
std::bind( &TestActionServer::handle_accepted, this, _1 ) );
}
46 std::shared_ptr<const typename ActionT::Goal> getCurrentGoal( ) const
{
return current_goal_;
}
51 void setReturnSuccess( bool return_success )
{
return_success_ = return_success;
}
56 bool getReturnSuccess( void )
{
return return_success_;
}
61 bool isGoalCancelled( )
{
return goal_cancelled_;
}
protected:
67 virtual rclcpp_action::GoalResponse handle_goal(
68 const rclcpp_action::GoalUUID &,
69 std::shared_ptr<const typename ActionT::Goal> goal )
{
current_goal_ = goal;
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
75 virtual rclcpp_action::CancelResponse handle_cancel(
const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> )
{
goal_cancelled_ = true;
return rclcpp_action::CancelResponse::ACCEPT;
}
82 virtual void execute(
const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> goal_handle ) = 0;
85 void handle_accepted(
const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> goal_handle )
{
using namespace std::placeholders; // NOLINT
// this needs to return quickly to avoid blocking the executor, so spin up a new thread
std::thread{std::bind( &TestActionServer::execute, this, _1 ), goal_handle}.detach( );
}
private:
typename rclcpp_action::Server<ActionT>::SharedPtr action_server_;
std::shared_ptr<const typename ActionT::Goal> current_goal_;
bool return_success_ = true;
bool goal_cancelled_ = false;
};
#endif // TEST_ACTION_SERVER_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TEST_BEHAVIOR_TREE_FIXTURE_HPP_
#define TEST_BEHAVIOR_TREE_FIXTURE_HPP_
#include <gtest/gtest.h>
#include <memory>
#include <set>
#include "behaviortree_cpp_v3/bt_factory.h"
#include "rclcpp/rclcpp.hpp"
#include "test_transform_handler.hpp"
#include "test_dummy_tree_node.hpp"
namespace nav2_behavior_tree
{
32 class BehaviorTreeTestFixture : public ::testing::Test
{
public:
35 static void SetUpTestCase( )
{
node_ = std::make_shared<rclcpp::Node>( "test_behavior_tree_fixture" );
transform_handler_ = std::make_shared<nav2_behavior_tree::TransformHandler>( node_ );
factory_ = std::make_shared<BT::BehaviorTreeFactory>( );
config_ = new BT::NodeConfiguration( );
// Create the blackboard that will be shared by all of the nodes in the tree
config_->blackboard = BT::Blackboard::create( );
// Put items on the blackboard
config_->blackboard->set<rclcpp::Node::SharedPtr>(
"node",
node_ );
config_->blackboard->set<std::shared_ptr<tf2_ros::Buffer>>(
"tf_buffer",
transform_handler_->getBuffer( ) );
config_->blackboard->set<std::chrono::milliseconds>(
"server_timeout",
std::chrono::milliseconds( 20 ) );
config_->blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration",
std::chrono::milliseconds( 10 ) );
config_->blackboard->set<bool>( "initial_pose_received", false );
transform_handler_->activate( );
transform_handler_->waitForTransform( );
}
64 static void TearDownTestCase( )
{
transform_handler_->deactivate( );
delete config_;
config_ = nullptr;
transform_handler_.reset( );
node_.reset( );
factory_.reset( );
}
protected:
75 static rclcpp::Node::SharedPtr node_;
76 static std::shared_ptr<nav2_behavior_tree::TransformHandler> transform_handler_;
77 static BT::NodeConfiguration * config_;
78 static std::shared_ptr<BT::BehaviorTreeFactory> factory_;
};
} // namespace nav2_behavior_tree
rclcpp::Node::SharedPtr nav2_behavior_tree::BehaviorTreeTestFixture::node_ = nullptr;
std::shared_ptr<nav2_behavior_tree::TransformHandler>
nav2_behavior_tree::BehaviorTreeTestFixture::transform_handler_ = nullptr;
BT::NodeConfiguration * nav2_behavior_tree::BehaviorTreeTestFixture::config_ = nullptr;
std::shared_ptr<BT::BehaviorTreeFactory>
nav2_behavior_tree::BehaviorTreeTestFixture::factory_ = nullptr;
#endif // TEST_BEHAVIOR_TREE_FIXTURE_HPP_
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <chrono>
#include <string>
#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "nav2_behavior_tree/bt_conversions.hpp"
template<typename T>
29 class TestNode : public BT::SyncActionNode
{
public:
32 TestNode( const std::string & name, const BT::NodeConfiguration & config )
: SyncActionNode( name, config )
{}
BT::NodeStatus tick( ) override
{
return BT::NodeStatus::SUCCESS;
}
static BT::PortsList providedPorts( )
{
return {
BT::InputPort<T>( "test" )
};
}
};
TEST( PointPortTest, test_wrong_syntax )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<PointPort test="1.0;2.0;3.0;4.0" />
</BehaviorTree>
</root> )";
BT::BehaviorTreeFactory factory;
factory.registerNodeType<TestNode<geometry_msgs::msg::Point>>( "PointPort" );
auto tree = factory.createTreeFromText( xml_txt );
geometry_msgs::msg::Point value;
tree.rootNode( )->getInput( "test", value );
EXPECT_EQ( value.x, 0.0 );
EXPECT_EQ( value.y, 0.0 );
EXPECT_EQ( value.z, 0.0 );
xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<PointPort test="1.0;2.0" />
</BehaviorTree>
</root> )";
tree = factory.createTreeFromText( xml_txt );
tree.rootNode( )->getInput( "test", value );
EXPECT_EQ( value.x, 0.0 );
EXPECT_EQ( value.y, 0.0 );
EXPECT_EQ( value.z, 0.0 );
}
TEST( PointPortTest, test_correct_syntax )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<PointPort test="1.0;2.0;3.0" />
</BehaviorTree>
</root> )";
BT::BehaviorTreeFactory factory;
factory.registerNodeType<TestNode<geometry_msgs::msg::Point>>( "PointPort" );
auto tree = factory.createTreeFromText( xml_txt );
geometry_msgs::msg::Point value;
tree.rootNode( )->getInput( "test", value );
EXPECT_EQ( value.x, 1.0 );
EXPECT_EQ( value.y, 2.0 );
EXPECT_EQ( value.z, 3.0 );
}
TEST( QuaternionPortTest, test_wrong_syntax )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<QuaternionPort test="1.0;2.0;3.0;4.0;5.0" />
</BehaviorTree>
</root> )";
BT::BehaviorTreeFactory factory;
factory.registerNodeType<TestNode<geometry_msgs::msg::Quaternion>>( "QuaternionPort" );
auto tree = factory.createTreeFromText( xml_txt );
geometry_msgs::msg::Quaternion value;
tree.rootNode( )->getInput( "test", value );
EXPECT_EQ( value.x, 0.0 );
EXPECT_EQ( value.y, 0.0 );
EXPECT_EQ( value.z, 0.0 );
EXPECT_EQ( value.w, 1.0 );
xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<QuaternionPort test="1.0;2.0;3.0" />
</BehaviorTree>
</root> )";
tree = factory.createTreeFromText( xml_txt );
tree.rootNode( )->getInput( "test", value );
EXPECT_EQ( value.x, 0.0 );
EXPECT_EQ( value.y, 0.0 );
EXPECT_EQ( value.z, 0.0 );
EXPECT_EQ( value.w, 1.0 );
}
TEST( QuaternionPortTest, test_correct_syntax )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<QuaternionPort test="0.7;0.0;0.0;0.7" />
</BehaviorTree>
</root> )";
BT::BehaviorTreeFactory factory;
factory.registerNodeType<TestNode<geometry_msgs::msg::Quaternion>>( "QuaternionPort" );
auto tree = factory.createTreeFromText( xml_txt );
geometry_msgs::msg::Quaternion value;
tree.rootNode( )->getInput( "test", value );
EXPECT_EQ( value.x, 0.7 );
EXPECT_EQ( value.y, 0.0 );
EXPECT_EQ( value.z, 0.0 );
EXPECT_EQ( value.w, 0.7 );
}
TEST( PoseStampedPortTest, test_wrong_syntax )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<PoseStampedPort test="0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0;8.0" />
</BehaviorTree>
</root> )";
BT::BehaviorTreeFactory factory;
factory.registerNodeType<TestNode<geometry_msgs::msg::PoseStamped>>( "PoseStampedPort" );
auto tree = factory.createTreeFromText( xml_txt );
geometry_msgs::msg::PoseStamped value;
tree.rootNode( )->getInput( "test", value );
EXPECT_EQ( rclcpp::Time( value.header.stamp ).nanoseconds( ), 0 );
EXPECT_EQ( value.header.frame_id, "" );
EXPECT_EQ( value.pose.position.x, 0.0 );
EXPECT_EQ( value.pose.position.y, 0.0 );
EXPECT_EQ( value.pose.position.z, 0.0 );
EXPECT_EQ( value.pose.orientation.x, 0.0 );
EXPECT_EQ( value.pose.orientation.y, 0.0 );
EXPECT_EQ( value.pose.orientation.z, 0.0 );
EXPECT_EQ( value.pose.orientation.w, 1.0 );
xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<PoseStampedPort test="0;map;1.0;2.0;3.0;4.0;5.0;6.0" />
</BehaviorTree>
</root> )";
tree = factory.createTreeFromText( xml_txt );
tree.rootNode( )->getInput( "test", value );
EXPECT_EQ( rclcpp::Time( value.header.stamp ).nanoseconds( ), 0 );
EXPECT_EQ( value.header.frame_id, "" );
EXPECT_EQ( value.pose.position.x, 0.0 );
EXPECT_EQ( value.pose.position.y, 0.0 );
EXPECT_EQ( value.pose.position.z, 0.0 );
EXPECT_EQ( value.pose.orientation.x, 0.0 );
EXPECT_EQ( value.pose.orientation.y, 0.0 );
EXPECT_EQ( value.pose.orientation.z, 0.0 );
EXPECT_EQ( value.pose.orientation.w, 1.0 );
}
TEST( PoseStampedPortTest, test_correct_syntax )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<PoseStampedPort test="0;map;1.0;2.0;3.0;4.0;5.0;6.0;7.0" />
</BehaviorTree>
</root> )";
BT::BehaviorTreeFactory factory;
factory.registerNodeType<TestNode<geometry_msgs::msg::PoseStamped>>( "PoseStampedPort" );
auto tree = factory.createTreeFromText( xml_txt );
tree = factory.createTreeFromText( xml_txt );
geometry_msgs::msg::PoseStamped value;
tree.rootNode( )->getInput( "test", value );
EXPECT_EQ( rclcpp::Time( value.header.stamp ).nanoseconds( ), 0 );
EXPECT_EQ( value.header.frame_id, "map" );
EXPECT_EQ( value.pose.position.x, 1.0 );
EXPECT_EQ( value.pose.position.y, 2.0 );
EXPECT_EQ( value.pose.position.z, 3.0 );
EXPECT_EQ( value.pose.orientation.x, 4.0 );
EXPECT_EQ( value.pose.orientation.y, 5.0 );
EXPECT_EQ( value.pose.orientation.z, 6.0 );
EXPECT_EQ( value.pose.orientation.w, 7.0 );
}
TEST( MillisecondsPortTest, test_correct_syntax )
{
std::string xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<MillisecondsPort test="10000" />
</BehaviorTree>
</root> )";
BT::BehaviorTreeFactory factory;
factory.registerNodeType<TestNode<std::chrono::milliseconds>>( "MillisecondsPort" );
auto tree = factory.createTreeFromText( xml_txt );
std::chrono::milliseconds value;
tree.rootNode( )->getInput( "test", value );
EXPECT_EQ( value.count( ), 10000 );
xml_txt =
R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<MillisecondsPort test="123.4" />
</BehaviorTree>
</root> )";
tree = factory.createTreeFromText( xml_txt );
tree.rootNode( )->getInput( "test", value );
EXPECT_EQ( value.count( ), 123 );
}
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TEST_DUMMY_TREE_NODE_HPP_
#define TEST_DUMMY_TREE_NODE_HPP_
#include <behaviortree_cpp_v3/basic_types.h>
#include <behaviortree_cpp_v3/action_node.h>
namespace nav2_behavior_tree
{
/**
* @brief A Dummy TreeNode to be used as a child for testing nodes
* Returns the current status on tick without any execution logic
*/
29 class DummyNode : public BT::ActionNodeBase
{
public:
32 DummyNode( )
: BT::ActionNodeBase( "dummy", {} )
{
}
37 void changeStatus( BT::NodeStatus status )
{
setStatus( status );
}
BT::NodeStatus executeTick( ) override
{
return tick( );
}
47 BT::NodeStatus tick( ) override
{
return status( );
}
52 void halt( ) override
{
}
};
} // namespace nav2_behavior_tree
#endif // TEST_DUMMY_TREE_NODE_HPP_
1 // Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TEST_SERVICE_HPP_
#define TEST_SERVICE_HPP_
#include <string>
#include <memory>
#include "rclcpp/rclcpp.hpp"
template<class ServiceT>
24 class TestService : public rclcpp::Node
{
public:
27 explicit TestService(
std::string service_name,
const rclcpp::NodeOptions & options = rclcpp::NodeOptions( ) )
: Node( "test_service", options )
{
using namespace std::placeholders; // NOLINT
server_ = create_service<ServiceT>(
service_name,
std::bind( &TestService::handle_service, this, _1, _2, _3 ) );
}
39 std::shared_ptr<typename ServiceT::Request> getCurrentRequest( ) const
{
return current_request_;
}
protected:
45 virtual void handle_service(
46 const std::shared_ptr<rmw_request_id_t> request_header,
47 const std::shared_ptr<typename ServiceT::Request> request,
48 const std::shared_ptr<typename ServiceT::Response> response )
{
( void )request_header;
( void )response;
current_request_ = request;
}
private:
typename rclcpp::Service<ServiceT>::SharedPtr server_;
std::shared_ptr<typename ServiceT::Request> current_request_;
};
#endif // TEST_SERVICE_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TEST_TRANSFORM_HANDLER_HPP_
#define TEST_TRANSFORM_HANDLER_HPP_
#include <memory>
#include <string>
#include <thread>
#include <chrono>
#include <algorithm>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_util/node_thread.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2_msgs/msg/tf_message.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
using namespace std::chrono_literals; // NOLINT
using namespace std::chrono; // NOLINT
namespace nav2_behavior_tree
{
40 class TransformHandler
{
public:
43 explicit TransformHandler( rclcpp::Node::SharedPtr & node )
: node_( node ),
is_active_( false ),
base_transform_( nullptr ),
tf_broadcaster_( nullptr )
{
tf_buffer_ = std::make_shared<tf2_ros::Buffer>( node_->get_clock( ) );
tf_listener_ = std::make_shared<tf2_ros::TransformListener>( *tf_buffer_ );
}
53 ~TransformHandler( )
{
if ( is_active_ ) {
deactivate( );
}
}
// Activate the tester before running tests
61 void activate( )
{
if ( is_active_ ) {
throw std::runtime_error( "Trying to activate while already active" );
}
is_active_ = true;
// Launch a thread to process the messages for this node
spin_thread_ = std::make_unique<nav2_util::NodeThread>( node_->get_node_base_interface( ) );
startRobotTransform( );
}
74 void deactivate( )
{
if ( !is_active_ ) {
throw std::runtime_error( "Trying to deactivate while already inactive" );
}
is_active_ = false;
spin_thread_.reset( );
tf_broadcaster_.reset( );
tf_buffer_.reset( );
tf_listener_.reset( );
}
86 std::shared_ptr<tf2_ros::Buffer> getBuffer( )
{
return tf_buffer_;
}
91 void waitForTransform( )
{
if ( is_active_ ) {
while ( !tf_buffer_->canTransform( "map", "base_link", rclcpp::Time( 0 ) ) ) {
std::this_thread::sleep_for( 100ms );
}
RCLCPP_INFO( node_->get_logger( ), "Transforms are available now!" );
return;
}
throw std::runtime_error( "Trying to deactivate while already inactive" );
}
103 void updateRobotPose( const geometry_msgs::msg::Pose & pose )
{
// Update base transform to publish
base_transform_->transform.translation.x = pose.position.x;
base_transform_->transform.translation.y = pose.position.y;
base_transform_->transform.translation.z = pose.position.z;
base_transform_->transform.rotation.x = pose.orientation.x;
base_transform_->transform.rotation.y = pose.orientation.y;
base_transform_->transform.rotation.z = pose.orientation.z;
base_transform_->transform.rotation.w = pose.orientation.w;
publishRobotTransform( );
}
private:
117 void publishRobotTransform( )
{
base_transform_->header.stamp = node_->now( );
tf_broadcaster_->sendTransform( *base_transform_ );
}
123 void startRobotTransform( )
{
// Provide the robot pose transform
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>( node_ );
if ( !base_transform_ ) {
base_transform_ = std::make_unique<geometry_msgs::msg::TransformStamped>( );
base_transform_->header.frame_id = "map";
base_transform_->child_frame_id = "base_link";
}
// Set an initial pose
geometry_msgs::msg::Pose robot_pose;
robot_pose.position.x = 0;
robot_pose.position.y = 0;
robot_pose.orientation.w = 1;
updateRobotPose( robot_pose );
// Publish the transform periodically
transform_timer_ = node_->create_wall_timer(
100ms, std::bind( &TransformHandler::publishRobotTransform, this ) );
}
146 rclcpp::Node::SharedPtr node_;
148 bool is_active_;
// A thread for spinning the ROS node
151 std::unique_ptr<nav2_util::NodeThread> spin_thread_;
// Subscriber
// The tester must provide the robot pose through a transform
156 std::unique_ptr<geometry_msgs::msg::TransformStamped> base_transform_;
157 std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
158 std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
159 std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
160 rclcpp::TimerBase::SharedPtr transform_timer_;
};
} // namespace nav2_behavior_tree
#endif // TEST_TRANSFORM_HANDLER_HPP_
1 // Copyright ( c ) 2018 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <chrono>
#include <string>
#include <memory>
#include <vector>
#include "nav2_util/lifecycle_node.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/create_timer_ros.h"
#include "pluginlib/class_loader.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "nav2_core/behavior.hpp"
#ifndef NAV2_BEHAVIORS__BEHAVIOR_SERVER_HPP_
#define NAV2_BEHAVIORS__BEHAVIOR_SERVER_HPP_
namespace behavior_server
{
/**
* @class behavior_server::BehaviorServer
* @brief An server hosting a map of behavior plugins
*/
38 class BehaviorServer : public nav2_util::LifecycleNode
{
public:
/**
* @brief A constructor for behavior_server::BehaviorServer
* @param options Additional options to control creation of the node.
*/
45 explicit BehaviorServer( const rclcpp::NodeOptions & options = rclcpp::NodeOptions( ) );
46 ~BehaviorServer( );
/**
* @brief Loads behavior plugins from parameter file
* @return bool if successfully loaded the plugins
*/
52 bool loadBehaviorPlugins( );
protected:
/**
* @brief Configure lifecycle server
*/
nav2_util::CallbackReturn on_configure( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Activate lifecycle server
*/
nav2_util::CallbackReturn on_activate( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Deactivate lifecycle server
*/
nav2_util::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Cleanup lifecycle server
*/
nav2_util::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Shutdown lifecycle server
*/
nav2_util::CallbackReturn on_shutdown( const rclcpp_lifecycle::State & state ) override;
std::shared_ptr<tf2_ros::Buffer> tf_;
std::shared_ptr<tf2_ros::TransformListener> transform_listener_;
// Plugins
pluginlib::ClassLoader<nav2_core::Behavior> plugin_loader_;
std::vector<pluginlib::UniquePtr<nav2_core::Behavior>> behaviors_;
std::vector<std::string> default_ids_;
std::vector<std::string> default_types_;
std::vector<std::string> behavior_ids_;
std::vector<std::string> behavior_types_;
// Utilities
std::unique_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
std::unique_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
};
} // namespace behavior_server
#endif // NAV2_BEHAVIORS__BEHAVIOR_SERVER_HPP_
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIORS__TIMED_BEHAVIOR_HPP_
#define NAV2_BEHAVIORS__TIMED_BEHAVIOR_HPP_
#include <memory>
#include <string>
#include <cmath>
#include <chrono>
#include <ctime>
#include <thread>
#include <utility>
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/create_timer_ros.h"
#include "geometry_msgs/msg/twist.hpp"
#include "nav2_util/simple_action_server.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav2_core/behavior.hpp"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpedantic"
#include "tf2/utils.h"
#pragma GCC diagnostic pop
namespace nav2_behaviors
{
41 enum class Status : int8_t
{
SUCCEEDED = 1,
FAILED = 2,
RUNNING = 3,
};
using namespace std::chrono_literals; //NOLINT
/**
* @class nav2_behaviors::Behavior
* @brief An action server Behavior base class implementing the action server and basic factory.
*/
template<typename ActionT>
55 class TimedBehavior : public nav2_core::Behavior
{
public:
using ActionServer = nav2_util::SimpleActionServer<ActionT>;
/**
* @brief A TimedBehavior constructor
*/
TimedBehavior( )
: action_server_( nullptr ),
cycle_frequency_( 10.0 ),
enabled_( false ),
transform_tolerance_( 0.0 )
{
}
virtual ~TimedBehavior( ) = default;
// Derived classes can override this method to catch the command and perform some checks
// before getting into the main loop. The method will only be called
// once and should return SUCCEEDED otherwise behavior will return FAILED.
76 virtual Status onRun( const std::shared_ptr<const typename ActionT::Goal> command ) = 0;
// This is the method derived classes should mainly implement
// and will be called cyclically while it returns RUNNING.
// Implement the behavior such that it runs some unit of work on each call
// and provides a status. The Behavior will finish once SUCCEEDED is returned
// It's up to the derived class to define the final commanded velocity.
virtual Status onCycleUpdate( ) = 0;
// an opportunity for derived classes to do something on configuration
// if they chose
virtual void onConfigure( )
{
}
// an opportunity for derived classes to do something on cleanup
// if they chose
virtual void onCleanup( )
{
}
// configure the server on lifecycle setup
void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
const std::string & name, std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker ) override
{
node_ = parent;
auto node = node_.lock( );
logger_ = node->get_logger( );
RCLCPP_INFO( logger_, "Configuring %s", name.c_str( ) );
behavior_name_ = name;
tf_ = tf;
node->get_parameter( "cycle_frequency", cycle_frequency_ );
node->get_parameter( "global_frame", global_frame_ );
node->get_parameter( "robot_base_frame", robot_base_frame_ );
node->get_parameter( "transform_tolerance", transform_tolerance_ );
action_server_ = std::make_shared<ActionServer>(
node, behavior_name_,
std::bind( &TimedBehavior::execute, this ) );
collision_checker_ = collision_checker;
vel_pub_ = node->template create_publisher<geometry_msgs::msg::Twist>( "cmd_vel", 1 );
onConfigure( );
}
// Cleanup server on lifecycle transition
void cleanup( ) override
{
action_server_.reset( );
vel_pub_.reset( );
onCleanup( );
}
// Activate server on lifecycle transition
void activate( ) override
{
RCLCPP_INFO( logger_, "Activating %s", behavior_name_.c_str( ) );
vel_pub_->on_activate( );
action_server_->activate( );
enabled_ = true;
}
// Deactivate server on lifecycle transition
void deactivate( ) override
{
vel_pub_->on_deactivate( );
action_server_->deactivate( );
enabled_ = false;
}
protected:
rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
std::string behavior_name_;
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_;
std::shared_ptr<ActionServer> action_server_;
std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
std::shared_ptr<tf2_ros::Buffer> tf_;
double cycle_frequency_;
double enabled_;
std::string global_frame_;
std::string robot_base_frame_;
double transform_tolerance_;
// Clock
rclcpp::Clock steady_clock_{RCL_STEADY_TIME};
// Logger
rclcpp::Logger logger_{rclcpp::get_logger( "nav2_behaviors" )};
// Main execution callbacks for the action server implementation calling the Behavior's
// onRun and cycle functions to execute a specific behavior
void execute( )
{
RCLCPP_INFO( logger_, "Attempting %s", behavior_name_.c_str( ) );
if ( !enabled_ ) {
RCLCPP_WARN(
logger_,
"Called while inactive, ignoring request." );
return;
}
if ( onRun( action_server_->get_current_goal( ) ) != Status::SUCCEEDED ) {
RCLCPP_INFO(
logger_,
"Initial checks failed for %s", behavior_name_.c_str( ) );
action_server_->terminate_current( );
return;
}
auto start_time = steady_clock_.now( );
// Initialize the ActionT result
auto result = std::make_shared<typename ActionT::Result>( );
rclcpp::WallRate loop_rate( cycle_frequency_ );
while ( rclcpp::ok( ) ) {
if ( action_server_->is_cancel_requested( ) ) {
RCLCPP_INFO( logger_, "Canceling %s", behavior_name_.c_str( ) );
stopRobot( );
result->total_elapsed_time = steady_clock_.now( ) - start_time;
action_server_->terminate_all( result );
return;
}
// TODO( orduno ) #868 Enable preempting a Behavior on-the-fly without stopping
if ( action_server_->is_preempt_requested( ) ) {
RCLCPP_ERROR(
logger_, "Received a preemption request for %s, "
" however feature is currently not implemented. Aborting and stopping.",
behavior_name_.c_str( ) );
stopRobot( );
result->total_elapsed_time = steady_clock_.now( ) - start_time;
action_server_->terminate_current( result );
return;
}
switch ( onCycleUpdate( ) ) {
case Status::SUCCEEDED:
RCLCPP_INFO(
logger_,
"%s completed successfully", behavior_name_.c_str( ) );
result->total_elapsed_time = steady_clock_.now( ) - start_time;
action_server_->succeeded_current( result );
return;
case Status::FAILED:
RCLCPP_WARN( logger_, "%s failed", behavior_name_.c_str( ) );
result->total_elapsed_time = steady_clock_.now( ) - start_time;
action_server_->terminate_current( result );
return;
case Status::RUNNING:
default:
loop_rate.sleep( );
break;
}
}
}
// Stop the robot with a commanded velocity
void stopRobot( )
{
auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>( );
cmd_vel->linear.x = 0.0;
cmd_vel->linear.y = 0.0;
cmd_vel->angular.z = 0.0;
vel_pub_->publish( std::move( cmd_vel ) );
}
};
} // namespace nav2_behaviors
#endif // NAV2_BEHAVIORS__TIMED_BEHAVIOR_HPP_
1 // Copyright ( c ) 2022 Joshua Wallace
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "back_up.hpp"
namespace nav2_behaviors
{
20 Status BackUp::onRun( const std::shared_ptr<const BackUpAction::Goal> command )
{
if ( command->target.y != 0.0 || command->target.z != 0.0 ) {
RCLCPP_INFO(
logger_,
"Backing up in Y and Z not supported, will only move in X." );
return Status::FAILED;
}
// Silently ensure that both the speed and direction are negative.
command_x_ = -std::fabs( command->target.x );
command_speed_ = -std::fabs( command->speed );
command_time_allowance_ = command->time_allowance;
end_time_ = steady_clock_.now( ) + command_time_allowance_;
if ( !nav2_util::getCurrentPose(
initial_pose_, *tf_, global_frame_, robot_base_frame_,
transform_tolerance_ ) )
{
RCLCPP_ERROR( logger_, "Initial robot pose is not available." );
return Status::FAILED;
}
return Status::SUCCEEDED;
}
} // namespace nav2_behaviors
#include "pluginlib/class_list_macros.hpp"
50 PLUGINLIB_EXPORT_CLASS( nav2_behaviors::BackUp, nav2_core::Behavior )
1 // Copyright ( c ) 2022 Joshua Wallace
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIORS__PLUGINS__BACK_UP_HPP_
#define NAV2_BEHAVIORS__PLUGINS__BACK_UP_HPP_
#include <memory>
#include "drive_on_heading.hpp"
#include "nav2_msgs/action/back_up.hpp"
using BackUpAction = nav2_msgs::action::BackUp;
namespace nav2_behaviors
{
28 class BackUp : public DriveOnHeading<nav2_msgs::action::BackUp>
{
public:
Status onRun( const std::shared_ptr<const BackUpAction::Goal> command ) override;
};
}
#endif // NAV2_BEHAVIORS__PLUGINS__BACK_UP_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2022 Joshua Wallace
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include "drive_on_heading.hpp"
#include "pluginlib/class_list_macros.hpp"
20 PLUGINLIB_EXPORT_CLASS( nav2_behaviors::DriveOnHeading<>, nav2_core::Behavior )
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2022 Joshua Wallace
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIORS__PLUGINS__DRIVE_ON_HEADING_HPP_
#define NAV2_BEHAVIORS__PLUGINS__DRIVE_ON_HEADING_HPP_
#include <chrono>
#include <memory>
#include <utility>
#include "nav2_behaviors/timed_behavior.hpp"
#include "nav2_msgs/action/drive_on_heading.hpp"
#include "nav2_msgs/action/back_up.hpp"
#include "nav2_util/node_utils.hpp"
namespace nav2_behaviors
{
/**
* @class nav2_behaviors::DriveOnHeading
* @brief An action server Behavior for spinning in
*/
template<typename ActionT = nav2_msgs::action::DriveOnHeading>
36 class DriveOnHeading : public TimedBehavior<ActionT>
{
public:
/**
* @brief A constructor for nav2_behaviors::DriveOnHeading
*/
DriveOnHeading( )
: TimedBehavior<ActionT>( ),
feedback_( std::make_shared<typename ActionT::Feedback>( ) ),
command_x_( 0.0 ),
command_speed_( 0.0 ),
simulate_ahead_time_( 0.0 )
{
}
~DriveOnHeading( ) = default;
/**
* @brief Initialization to run behavior
* @param command Goal to execute
* @return Status of behavior
*/
58 Status onRun( const std::shared_ptr<const typename ActionT::Goal> command ) override
{
if ( command->target.y != 0.0 || command->target.z != 0.0 ) {
RCLCPP_INFO(
this->logger_,
"DrivingOnHeading in Y and Z not supported, will only move in X." );
return Status::FAILED;
}
// Ensure that both the speed and direction have the same sign
if ( !( ( command->target.x > 0.0 ) == ( command->speed > 0.0 ) ) ) {
RCLCPP_ERROR( this->logger_, "Speed and command sign did not match" );
return Status::FAILED;
}
command_x_ = command->target.x;
command_speed_ = command->speed;
command_time_allowance_ = command->time_allowance;
end_time_ = this->steady_clock_.now( ) + command_time_allowance_;
if ( !nav2_util::getCurrentPose(
initial_pose_, *this->tf_, this->global_frame_, this->robot_base_frame_,
this->transform_tolerance_ ) )
{
RCLCPP_ERROR( this->logger_, "Initial robot pose is not available." );
return Status::FAILED;
}
return Status::SUCCEEDED;
}
/**
* @brief Loop function to run behavior
* @return Status of behavior
*/
94 Status onCycleUpdate( )
{
rclcpp::Duration time_remaining = end_time_ - this->steady_clock_.now( );
if ( time_remaining.seconds( ) < 0.0 && command_time_allowance_.seconds( ) > 0.0 ) {
this->stopRobot( );
RCLCPP_WARN(
this->logger_,
"Exceeded time allowance before reaching the DriveOnHeading goal - Exiting DriveOnHeading" );
return Status::FAILED;
}
geometry_msgs::msg::PoseStamped current_pose;
if ( !nav2_util::getCurrentPose(
current_pose, *this->tf_, this->global_frame_, this->robot_base_frame_,
this->transform_tolerance_ ) )
{
RCLCPP_ERROR( this->logger_, "Current robot pose is not available." );
return Status::FAILED;
}
double diff_x = initial_pose_.pose.position.x - current_pose.pose.position.x;
double diff_y = initial_pose_.pose.position.y - current_pose.pose.position.y;
double distance = hypot( diff_x, diff_y );
feedback_->distance_traveled = distance;
this->action_server_->publish_feedback( feedback_ );
if ( distance >= std::fabs( command_x_ ) ) {
this->stopRobot( );
return Status::SUCCEEDED;
}
auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>( );
cmd_vel->linear.y = 0.0;
cmd_vel->angular.z = 0.0;
cmd_vel->linear.x = command_speed_;
geometry_msgs::msg::Pose2D pose2d;
pose2d.x = current_pose.pose.position.x;
pose2d.y = current_pose.pose.position.y;
pose2d.theta = tf2::getYaw( current_pose.pose.orientation );
if ( !isCollisionFree( distance, cmd_vel.get( ), pose2d ) ) {
this->stopRobot( );
RCLCPP_WARN( this->logger_, "Collision Ahead - Exiting DriveOnHeading" );
return Status::FAILED;
}
this->vel_pub_->publish( std::move( cmd_vel ) );
return Status::RUNNING;
}
protected:
/**
* @brief Check if pose is collision free
* @param distance Distance to check forward
* @param cmd_vel current commanded velocity
* @param pose2d Current pose
* @return is collision free or not
*/
155 bool isCollisionFree(
const double & distance,
geometry_msgs::msg::Twist * cmd_vel,
geometry_msgs::msg::Pose2D & pose2d )
{
// Simulate ahead by simulate_ahead_time_ in this->cycle_frequency_ increments
int cycle_count = 0;
double sim_position_change;
const double diff_dist = abs( command_x_ ) - distance;
const int max_cycle_count = static_cast<int>( this->cycle_frequency_ * simulate_ahead_time_ );
geometry_msgs::msg::Pose2D init_pose = pose2d;
bool fetch_data = true;
while ( cycle_count < max_cycle_count ) {
sim_position_change = cmd_vel->linear.x * ( cycle_count / this->cycle_frequency_ );
pose2d.x = init_pose.x + sim_position_change * cos( init_pose.theta );
pose2d.y = init_pose.y + sim_position_change * sin( init_pose.theta );
cycle_count++;
if ( diff_dist - abs( sim_position_change ) <= 0. ) {
break;
}
if ( !this->collision_checker_->isCollisionFree( pose2d, fetch_data ) ) {
return false;
}
fetch_data = false;
}
return true;
}
/**
* @brief Configuration of behavior action
*/
189 void onConfigure( ) override
{
auto node = this->node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
nav2_util::declare_parameter_if_not_declared(
node,
"simulate_ahead_time", rclcpp::ParameterValue( 2.0 ) );
node->get_parameter( "simulate_ahead_time", simulate_ahead_time_ );
}
typename ActionT::Feedback::SharedPtr feedback_;
geometry_msgs::msg::PoseStamped initial_pose_;
double command_x_;
double command_speed_;
rclcpp::Duration command_time_allowance_{0, 0};
rclcpp::Time end_time_;
double simulate_ahead_time_;
};
} // namespace nav2_behaviors
#endif // NAV2_BEHAVIORS__PLUGINS__DRIVE_ON_HEADING_HPP_
1 // Copyright ( c ) 2018 Intel Corporation, 2019 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <cmath>
#include <thread>
#include <algorithm>
#include <memory>
#include <utility>
#include "spin.hpp"
#include "tf2/utils.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "nav2_util/node_utils.hpp"
using namespace std::chrono_literals;
namespace nav2_behaviors
{
31 Spin::Spin( )
: TimedBehavior<SpinAction>( ),
feedback_( std::make_shared<SpinAction::Feedback>( ) ),
min_rotational_vel_( 0.0 ),
max_rotational_vel_( 0.0 ),
rotational_acc_lim_( 0.0 ),
cmd_yaw_( 0.0 ),
prev_yaw_( 0.0 ),
relative_yaw_( 0.0 ),
simulate_ahead_time_( 0.0 )
{
}
44 Spin::~Spin( ) = default;
46 void Spin::onConfigure( )
{
auto node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
nav2_util::declare_parameter_if_not_declared(
node,
"simulate_ahead_time", rclcpp::ParameterValue( 2.0 ) );
node->get_parameter( "simulate_ahead_time", simulate_ahead_time_ );
nav2_util::declare_parameter_if_not_declared(
node,
"max_rotational_vel", rclcpp::ParameterValue( 1.0 ) );
node->get_parameter( "max_rotational_vel", max_rotational_vel_ );
nav2_util::declare_parameter_if_not_declared(
node,
"min_rotational_vel", rclcpp::ParameterValue( 0.4 ) );
node->get_parameter( "min_rotational_vel", min_rotational_vel_ );
nav2_util::declare_parameter_if_not_declared(
node,
"rotational_acc_lim", rclcpp::ParameterValue( 3.2 ) );
node->get_parameter( "rotational_acc_lim", rotational_acc_lim_ );
}
74 Status Spin::onRun( const std::shared_ptr<const SpinAction::Goal> command )
{
geometry_msgs::msg::PoseStamped current_pose;
if ( !nav2_util::getCurrentPose(
current_pose, *tf_, global_frame_, robot_base_frame_,
transform_tolerance_ ) )
{
RCLCPP_ERROR( logger_, "Current robot pose is not available." );
return Status::FAILED;
}
prev_yaw_ = tf2::getYaw( current_pose.pose.orientation );
relative_yaw_ = 0.0;
cmd_yaw_ = command->target_yaw;
RCLCPP_INFO(
logger_, "Turning %0.2f for spin behavior.",
cmd_yaw_ );
command_time_allowance_ = command->time_allowance;
end_time_ = steady_clock_.now( ) + command_time_allowance_;
return Status::SUCCEEDED;
}
99 Status Spin::onCycleUpdate( )
{
rclcpp::Duration time_remaining = end_time_ - steady_clock_.now( );
if ( time_remaining.seconds( ) < 0.0 && command_time_allowance_.seconds( ) > 0.0 ) {
stopRobot( );
RCLCPP_WARN(
logger_,
"Exceeded time allowance before reaching the Spin goal - Exiting Spin" );
return Status::FAILED;
}
geometry_msgs::msg::PoseStamped current_pose;
if ( !nav2_util::getCurrentPose(
current_pose, *tf_, global_frame_, robot_base_frame_,
transform_tolerance_ ) )
{
RCLCPP_ERROR( logger_, "Current robot pose is not available." );
return Status::FAILED;
}
const double current_yaw = tf2::getYaw( current_pose.pose.orientation );
double delta_yaw = current_yaw - prev_yaw_;
if ( abs( delta_yaw ) > M_PI ) {
delta_yaw = copysign( 2 * M_PI - abs( delta_yaw ), prev_yaw_ );
}
relative_yaw_ += delta_yaw;
prev_yaw_ = current_yaw;
feedback_->angular_distance_traveled = static_cast<float>( relative_yaw_ );
action_server_->publish_feedback( feedback_ );
double remaining_yaw = abs( cmd_yaw_ ) - abs( relative_yaw_ );
if ( remaining_yaw < 1e-6 ) {
stopRobot( );
return Status::SUCCEEDED;
}
double vel = sqrt( 2 * rotational_acc_lim_ * remaining_yaw );
vel = std::min( std::max( vel, min_rotational_vel_ ), max_rotational_vel_ );
auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>( );
cmd_vel->angular.z = copysign( vel, cmd_yaw_ );
geometry_msgs::msg::Pose2D pose2d;
pose2d.x = current_pose.pose.position.x;
pose2d.y = current_pose.pose.position.y;
pose2d.theta = tf2::getYaw( current_pose.pose.orientation );
if ( !isCollisionFree( relative_yaw_, cmd_vel.get( ), pose2d ) ) {
stopRobot( );
RCLCPP_WARN( logger_, "Collision Ahead - Exiting Spin" );
return Status::FAILED;
}
vel_pub_->publish( std::move( cmd_vel ) );
return Status::RUNNING;
}
160 bool Spin::isCollisionFree(
const double & relative_yaw,
162 geometry_msgs::msg::Twist * cmd_vel,
163 geometry_msgs::msg::Pose2D & pose2d )
{
// Simulate ahead by simulate_ahead_time_ in cycle_frequency_ increments
int cycle_count = 0;
double sim_position_change;
const int max_cycle_count = static_cast<int>( cycle_frequency_ * simulate_ahead_time_ );
geometry_msgs::msg::Pose2D init_pose = pose2d;
bool fetch_data = true;
while ( cycle_count < max_cycle_count ) {
sim_position_change = cmd_vel->angular.z * ( cycle_count / cycle_frequency_ );
pose2d.theta = init_pose.theta + sim_position_change;
cycle_count++;
if ( abs( relative_yaw ) - abs( sim_position_change ) <= 0. ) {
break;
}
if ( !collision_checker_->isCollisionFree( pose2d, fetch_data ) ) {
return false;
}
fetch_data = false;
}
return true;
}
} // namespace nav2_behaviors
#include "pluginlib/class_list_macros.hpp"
192 PLUGINLIB_EXPORT_CLASS( nav2_behaviors::Spin, nav2_core::Behavior )
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIORS__PLUGINS__SPIN_HPP_
#define NAV2_BEHAVIORS__PLUGINS__SPIN_HPP_
#include <chrono>
#include <string>
#include <memory>
#include "nav2_behaviors/timed_behavior.hpp"
#include "nav2_msgs/action/spin.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
namespace nav2_behaviors
{
using SpinAction = nav2_msgs::action::Spin;
/**
* @class nav2_behaviors::Spin
* @brief An action server behavior for spinning in
*/
34 class Spin : public TimedBehavior<SpinAction>
{
public:
/**
* @brief A constructor for nav2_behaviors::Spin
*/
40 Spin( );
41 ~Spin( );
/**
* @brief Initialization to run behavior
* @param command Goal to execute
* @return Status of behavior
*/
Status onRun( const std::shared_ptr<const SpinAction::Goal> command ) override;
/**
* @brief Configuration of behavior action
*/
void onConfigure( ) override;
/**
* @brief Loop function to run behavior
* @return Status of behavior
*/
Status onCycleUpdate( ) override;
protected:
/**
* @brief Check if pose is collision free
* @param distance Distance to check forward
* @param cmd_vel current commanded velocity
* @param pose2d Current pose
* @return is collision free or not
*/
bool isCollisionFree(
const double & distance,
geometry_msgs::msg::Twist * cmd_vel,
geometry_msgs::msg::Pose2D & pose2d );
SpinAction::Feedback::SharedPtr feedback_;
double min_rotational_vel_;
double max_rotational_vel_;
double rotational_acc_lim_;
double cmd_yaw_;
double prev_yaw_;
double relative_yaw_;
double simulate_ahead_time_;
rclcpp::Duration command_time_allowance_{0, 0};
rclcpp::Time end_time_;
};
} // namespace nav2_behaviors
#endif // NAV2_BEHAVIORS__PLUGINS__SPIN_HPP_
1 // Copyright ( c ) 2019 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <chrono>
#include <memory>
#include "wait.hpp"
namespace nav2_behaviors
{
23 Wait::Wait( )
: TimedBehavior<WaitAction>( ),
feedback_( std::make_shared<WaitAction::Feedback>( ) )
{
}
29 Wait::~Wait( ) = default;
31 Status Wait::onRun( const std::shared_ptr<const WaitAction::Goal> command )
{
wait_end_ = std::chrono::steady_clock::now( ) +
rclcpp::Duration( command->time ).to_chrono<std::chrono::nanoseconds>( );
return Status::SUCCEEDED;
}
38 Status Wait::onCycleUpdate( )
{
auto current_point = std::chrono::steady_clock::now( );
auto time_left =
std::chrono::duration_cast<std::chrono::nanoseconds>( wait_end_ - current_point ).count( );
feedback_->time_left = rclcpp::Duration( rclcpp::Duration::from_nanoseconds( time_left ) );
action_server_->publish_feedback( feedback_ );
if ( time_left > 0 ) {
return Status::RUNNING;
} else {
return Status::SUCCEEDED;
}
}
} // namespace nav2_behaviors
#include "pluginlib/class_list_macros.hpp"
57 PLUGINLIB_EXPORT_CLASS( nav2_behaviors::Wait, nav2_core::Behavior )
1 // Copyright ( c ) 2019 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BEHAVIORS__PLUGINS__WAIT_HPP_
#define NAV2_BEHAVIORS__PLUGINS__WAIT_HPP_
#include <chrono>
#include <string>
#include <memory>
#include "nav2_behaviors/timed_behavior.hpp"
#include "nav2_msgs/action/wait.hpp"
namespace nav2_behaviors
{
using WaitAction = nav2_msgs::action::Wait;
/**
* @class nav2_behaviors::Wait
* @brief An action server behavior for waiting a fixed duration
*/
33 class Wait : public TimedBehavior<WaitAction>
{
public:
/**
* @brief A constructor for nav2_behaviors::Wait
*/
39 Wait( );
40 ~Wait( );
/**
* @brief Initialization to run behavior
* @param command Goal to execute
* @return Status of behavior
*/
Status onRun( const std::shared_ptr<const WaitAction::Goal> command ) override;
/**
* @brief Loop function to run behavior
* @return Status of behavior
*/
Status onCycleUpdate( ) override;
protected:
std::chrono::time_point<std::chrono::steady_clock> wait_end_;
WaitAction::Feedback::SharedPtr feedback_;
};
} // namespace nav2_behaviors
#endif // NAV2_BEHAVIORS__PLUGINS__WAIT_HPP_
// Copyright ( c ) 2018 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <memory>
#include <string>
#include <vector>
#include <utility>
#include "nav2_util/node_utils.hpp"
#include "nav2_behaviors/behavior_server.hpp"
namespace behavior_server
{
25 BehaviorServer::BehaviorServer( const rclcpp::NodeOptions & options )
: LifecycleNode( "behavior_server", "", options ),
plugin_loader_( "nav2_core", "nav2_core::Behavior" ),
default_ids_{"spin", "backup", "drive_on_heading", "wait"},
29 default_types_{"nav2_behaviors/Spin",
"nav2_behaviors/BackUp",
"nav2_behaviors/DriveOnHeading",
"nav2_behaviors/Wait"}
{
34 declare_parameter(
"costmap_topic",
rclcpp::ParameterValue( std::string( "local_costmap/costmap_raw" ) ) );
37 declare_parameter(
"footprint_topic",
rclcpp::ParameterValue( std::string( "local_costmap/published_footprint" ) ) );
40 declare_parameter( "cycle_frequency", rclcpp::ParameterValue( 10.0 ) );
41 declare_parameter( "behavior_plugins", default_ids_ );
43 get_parameter( "behavior_plugins", behavior_ids_ );
if ( behavior_ids_ == default_ids_ ) {
for ( size_t i = 0; i < default_ids_.size( ); ++i ) {
declare_parameter( default_ids_[i] + ".plugin", default_types_[i] );
}
}
declare_parameter(
"global_frame",
rclcpp::ParameterValue( std::string( "odom" ) ) );
declare_parameter(
"robot_base_frame",
rclcpp::ParameterValue( std::string( "base_link" ) ) );
declare_parameter(
"transform_tolerance",
rclcpp::ParameterValue( 0.1 ) );
}
BehaviorServer::~BehaviorServer( )
{
behaviors_.clear( );
}
nav2_util::CallbackReturn
BehaviorServer::on_configure( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Configuring" );
tf_ = std::make_shared<tf2_ros::Buffer>( get_clock( ) );
auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
get_node_base_interface( ),
get_node_timers_interface( ) );
tf_->setCreateTimerInterface( timer_interface );
transform_listener_ = std::make_shared<tf2_ros::TransformListener>( *tf_ );
std::string costmap_topic, footprint_topic, robot_base_frame;
double transform_tolerance;
this->get_parameter( "costmap_topic", costmap_topic );
this->get_parameter( "footprint_topic", footprint_topic );
this->get_parameter( "transform_tolerance", transform_tolerance );
this->get_parameter( "robot_base_frame", robot_base_frame );
costmap_sub_ = std::make_unique<nav2_costmap_2d::CostmapSubscriber>(
shared_from_this( ), costmap_topic );
footprint_sub_ = std::make_unique<nav2_costmap_2d::FootprintSubscriber>(
shared_from_this( ), footprint_topic, *tf_, robot_base_frame, transform_tolerance );
collision_checker_ = std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(
*costmap_sub_, *footprint_sub_, this->get_name( ) );
behavior_types_.resize( behavior_ids_.size( ) );
if ( !loadBehaviorPlugins( ) ) {
return nav2_util::CallbackReturn::FAILURE;
}
return nav2_util::CallbackReturn::SUCCESS;
}
bool
BehaviorServer::loadBehaviorPlugins( )
{
auto node = shared_from_this( );
for ( size_t i = 0; i != behavior_ids_.size( ); i++ ) {
behavior_types_[i] = nav2_util::get_plugin_type_param( node, behavior_ids_[i] );
try {
RCLCPP_INFO(
get_logger( ), "Creating behavior plugin %s of type %s",
behavior_ids_[i].c_str( ), behavior_types_[i].c_str( ) );
behaviors_.push_back( plugin_loader_.createUniqueInstance( behavior_types_[i] ) );
behaviors_.back( )->configure( node, behavior_ids_[i], tf_, collision_checker_ );
} catch ( const pluginlib::PluginlibException & ex ) {
RCLCPP_FATAL(
get_logger( ), "Failed to create behavior %s of type %s."
" Exception: %s", behavior_ids_[i].c_str( ), behavior_types_[i].c_str( ),
ex.what( ) );
return false;
}
}
return true;
}
nav2_util::CallbackReturn
BehaviorServer::on_activate( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Activating" );
std::vector<pluginlib::UniquePtr<nav2_core::Behavior>>::iterator iter;
for ( iter = behaviors_.begin( ); iter != behaviors_.end( ); ++iter ) {
( *iter )->activate( );
}
// create bond connection
createBond( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
BehaviorServer::on_deactivate( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Deactivating" );
std::vector<pluginlib::UniquePtr<nav2_core::Behavior>>::iterator iter;
for ( iter = behaviors_.begin( ); iter != behaviors_.end( ); ++iter ) {
( *iter )->deactivate( );
}
// destroy bond connection
destroyBond( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
BehaviorServer::on_cleanup( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Cleaning up" );
std::vector<pluginlib::UniquePtr<nav2_core::Behavior>>::iterator iter;
for ( iter = behaviors_.begin( ); iter != behaviors_.end( ); ++iter ) {
( *iter )->cleanup( );
}
behaviors_.clear( );
transform_listener_.reset( );
tf_.reset( );
footprint_sub_.reset( );
costmap_sub_.reset( );
collision_checker_.reset( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
BehaviorServer::on_shutdown( const rclcpp_lifecycle::State & )
{
RCLCPP_INFO( get_logger( ), "Shutting down" );
return nav2_util::CallbackReturn::SUCCESS;
}
} // end namespace behavior_server
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE( behavior_server::BehaviorServer )
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2019 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <memory>
#include "nav2_behaviors/behavior_server.hpp"
#include "rclcpp/rclcpp.hpp"
21 int main( int argc, char ** argv )
{
rclcpp::init( argc, argv );
auto recoveries_node = std::make_shared<behavior_server::BehaviorServer>( );
rclcpp::spin( recoveries_node->get_node_base_interface( ) );
rclcpp::shutdown( );
return 0;
}
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <string>
#include <memory>
#include <chrono>
#include <iostream>
#include <thread>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_behaviors/timed_behavior.hpp"
#include "nav2_msgs/action/dummy_behavior.hpp"
using nav2_behaviors::TimedBehavior;
using nav2_behaviors::Status;
using BehaviorAction = nav2_msgs::action::DummyBehavior;
using ClientGoalHandle = rclcpp_action::ClientGoalHandle<BehaviorAction>;
using namespace std::chrono_literals;
// A behavior for testing the base class
37 class DummyBehavior : public TimedBehavior<BehaviorAction>
{
public:
40 DummyBehavior( )
: TimedBehavior<BehaviorAction>( ),
initialized_( false ) {}
~DummyBehavior( ) = default;
Status onRun( const std::shared_ptr<const BehaviorAction::Goal> goal ) override
{
// A normal behavior would catch the command and initialize
initialized_ = false;
command_ = goal->command.data;
start_time_ = std::chrono::system_clock::now( );
// onRun method can have various possible outcomes ( success, failure, cancelled )
// The output is defined by the tester class on the command string.
if ( command_ == "Testing success" || command_ == "Testing failure on run" ) {
initialized_ = true;
return Status::SUCCEEDED;
}
return Status::FAILED;
}
Status onCycleUpdate( ) override
{
// A normal behavior would set the robot in motion in the first call
// and check for robot states on subsequent calls to check if the movement
// was completed.
if ( command_ != "Testing success" || !initialized_ ) {
return Status::FAILED;
}
// For testing, pretend the robot takes some fixed
// amount of time to complete the motion.
auto current_time = std::chrono::system_clock::now( );
auto motion_duration = 1s;
if ( current_time - start_time_ >= motion_duration ) {
// Movement was completed
return Status::SUCCEEDED;
}
return Status::RUNNING;
}
private:
bool initialized_;
std::string command_;
std::chrono::system_clock::time_point start_time_;
};
// Define a test class to hold the context for the tests
class BehaviorTest : public ::testing::Test
{
protected:
BehaviorTest( ) {SetUp( );}
~BehaviorTest( ) = default;
void SetUp( ) override
{
node_lifecycle_ =
std::make_shared<rclcpp_lifecycle::LifecycleNode>(
"LifecycleBehaviorTestNode", rclcpp::NodeOptions( ) );
node_lifecycle_->declare_parameter(
"costmap_topic",
rclcpp::ParameterValue( std::string( "local_costmap/costmap_raw" ) ) );
node_lifecycle_->declare_parameter(
"footprint_topic",
rclcpp::ParameterValue( std::string( "local_costmap/published_footprint" ) ) );
tf_buffer_ = std::make_shared<tf2_ros::Buffer>( node_lifecycle_->get_clock( ) );
auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
node_lifecycle_->get_node_base_interface( ),
node_lifecycle_->get_node_timers_interface( ) );
tf_buffer_->setCreateTimerInterface( timer_interface );
tf_listener_ = std::make_shared<tf2_ros::TransformListener>( *tf_buffer_ );
std::string costmap_topic, footprint_topic;
node_lifecycle_->get_parameter( "costmap_topic", costmap_topic );
node_lifecycle_->get_parameter( "footprint_topic", footprint_topic );
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_ =
std::make_shared<nav2_costmap_2d::CostmapSubscriber>(
node_lifecycle_, costmap_topic );
std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_ =
std::make_shared<nav2_costmap_2d::FootprintSubscriber>(
node_lifecycle_, footprint_topic, *tf_buffer_ );
std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_ =
std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(
*costmap_sub_, *footprint_sub_,
node_lifecycle_->get_name( ) );
behavior_ = std::make_shared<DummyBehavior>( );
behavior_->configure( node_lifecycle_, "Behavior", tf_buffer_, collision_checker_ );
behavior_->activate( );
client_ = rclcpp_action::create_client<BehaviorAction>(
node_lifecycle_->get_node_base_interface( ),
node_lifecycle_->get_node_graph_interface( ),
node_lifecycle_->get_node_logging_interface( ),
node_lifecycle_->get_node_waitables_interface( ), "Behavior" );
std::cout << "Setup complete." << std::endl;
}
void TearDown( ) override {}
bool sendCommand( const std::string & command )
{
if ( !client_->wait_for_action_server( 4s ) ) {
std::cout << "Server not up" << std::endl;
return false;
}
auto goal = BehaviorAction::Goal( );
goal.command.data = command;
auto future_goal = client_->async_send_goal( goal );
if ( rclcpp::spin_until_future_complete( node_lifecycle_, future_goal ) !=
rclcpp::FutureReturnCode::SUCCESS )
{
std::cout << "failed sending goal" << std::endl;
// failed sending the goal
return false;
}
goal_handle_ = future_goal.get( );
if ( !goal_handle_ ) {
std::cout << "goal was rejected" << std::endl;
// goal was rejected by the action server
return false;
}
return true;
}
Status getOutcome( )
{
if ( getResult( ).code == rclcpp_action::ResultCode::SUCCEEDED ) {
return Status::SUCCEEDED;
}
return Status::FAILED;
}
ClientGoalHandle::WrappedResult getResult( )
{
std::cout << "Getting async result..." << std::endl;
auto future_result = client_->async_get_result( goal_handle_ );
std::cout << "Waiting on future..." << std::endl;
rclcpp::spin_until_future_complete( node_lifecycle_, future_result );
std::cout << "future received!" << std::endl;
return future_result.get( );
}
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_lifecycle_;
std::shared_ptr<DummyBehavior> behavior_;
std::shared_ptr<rclcpp_action::Client<BehaviorAction>> client_;
std::shared_ptr<rclcpp_action::ClientGoalHandle<BehaviorAction>> goal_handle_;
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
};
// Define the tests
TEST_F( BehaviorTest, testingSuccess )
{
ASSERT_TRUE( sendCommand( "Testing success" ) );
EXPECT_EQ( getOutcome( ), Status::SUCCEEDED );
SUCCEED( );
}
TEST_F( BehaviorTest, testingFailureOnRun )
{
ASSERT_TRUE( sendCommand( "Testing failure on run" ) );
EXPECT_EQ( getOutcome( ), Status::FAILED );
SUCCEED( );
}
TEST_F( BehaviorTest, testingFailureOnInit )
{
ASSERT_TRUE( sendCommand( "Testing failure on init" ) );
EXPECT_EQ( getOutcome( ), Status::FAILED );
SUCCEED( );
}
TEST_F( BehaviorTest, testingSequentialFailures )
{
ASSERT_TRUE( sendCommand( "Testing failure on run" ) );
EXPECT_EQ( getOutcome( ), Status::FAILED );
SUCCEED( );
}
TEST_F( BehaviorTest, testingTotalElapsedTimeIsGratherThanZeroIfStarted )
{
ASSERT_TRUE( sendCommand( "Testing success" ) );
EXPECT_GT( getResult( ).result->total_elapsed_time.sec, 0.0 );
SUCCEED( );
}
TEST_F( BehaviorTest, testingTotalElapsedTimeIsZeroIfFailureOnInit )
{
ASSERT_TRUE( sendCommand( "Testing failure on init" ) );
EXPECT_EQ( getResult( ).result->total_elapsed_time.sec, 0.0 );
SUCCEED( );
}
TEST_F( BehaviorTest, testingTotalElapsedTimeIsZeroIfFailureOnRun )
{
ASSERT_TRUE( sendCommand( "Testing failure on run" ) );
EXPECT_EQ( getResult( ).result->total_elapsed_time.sec, 0.0 );
SUCCEED( );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BT_NAVIGATOR__BT_NAVIGATOR_HPP_
#define NAV2_BT_NAVIGATOR__BT_NAVIGATOR_HPP_
#include <memory>
#include <string>
#include <vector>
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/odometry_utils.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/create_timer_ros.h"
#include "nav2_bt_navigator/navigators/navigate_to_pose.hpp"
#include "nav2_bt_navigator/navigators/navigate_through_poses.hpp"
namespace nav2_bt_navigator
{
/**
* @class nav2_bt_navigator::BtNavigator
* @brief An action server that uses behavior tree for navigating a robot to its
* goal position.
*/
39 class BtNavigator : public nav2_util::LifecycleNode
{
public:
/**
* @brief A constructor for nav2_bt_navigator::BtNavigator class
* @param options Additional options to control creation of the node.
*/
46 explicit BtNavigator( const rclcpp::NodeOptions & options = rclcpp::NodeOptions( ) );
/**
* @brief A destructor for nav2_bt_navigator::BtNavigator class
*/
50 ~BtNavigator( );
protected:
/**
* @brief Configures member variables
*
* Initializes action server for "NavigationToPose"; subscription to
* "goal_sub"; and builds behavior tree from xml file.
* @param state Reference to LifeCycle node state
* @return SUCCESS or FAILURE
*/
nav2_util::CallbackReturn on_configure( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Activates action server
* @param state Reference to LifeCycle node state
* @return SUCCESS or FAILURE
*/
nav2_util::CallbackReturn on_activate( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Deactivates action server
* @param state Reference to LifeCycle node state
* @return SUCCESS or FAILURE
*/
nav2_util::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Resets member variables
* @param state Reference to LifeCycle node state
* @return SUCCESS or FAILURE
*/
nav2_util::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Called when in shutdown state
* @param state Reference to LifeCycle node state
* @return SUCCESS or FAILURE
*/
nav2_util::CallbackReturn on_shutdown( const rclcpp_lifecycle::State & state ) override;
// To handle all the BT related execution
std::unique_ptr<nav2_bt_navigator::Navigator<nav2_msgs::action::NavigateToPose>> pose_navigator_;
std::unique_ptr<nav2_bt_navigator::Navigator<nav2_msgs::action::NavigateThroughPoses>>
poses_navigator_;
nav2_bt_navigator::NavigatorMuxer plugin_muxer_;
// Odometry smoother object
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
// Metrics for feedback
std::string robot_frame_;
std::string global_frame_;
double transform_tolerance_;
std::string odom_topic_;
// Spinning transform that can be used by the BT nodes
std::shared_ptr<tf2_ros::Buffer> tf_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
};
} // namespace nav2_bt_navigator
#endif // NAV2_BT_NAVIGATOR__BT_NAVIGATOR_HPP_
// Copyright ( c ) 2021 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BT_NAVIGATOR__NAVIGATOR_HPP_
#define NAV2_BT_NAVIGATOR__NAVIGATOR_HPP_
#include <memory>
#include <string>
#include <vector>
#include <mutex>
#include "nav2_util/odometry_utils.hpp"
#include "tf2_ros/buffer.h"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "pluginlib/class_loader.hpp"
#include "nav2_behavior_tree/bt_action_server.hpp"
namespace nav2_bt_navigator
{
/**
* @struct FeedbackUtils
* @brief Navigator feedback utilities required to get transforms and reference frames.
*/
struct FeedbackUtils
{
std::string robot_frame;
std::string global_frame;
double transform_tolerance;
std::shared_ptr<tf2_ros::Buffer> tf;
};
/**
* @class NavigatorMuxer
* @brief A class to control the state of the BT navigator by allowing only a single
* plugin to be processed at a time.
*/
50 class NavigatorMuxer
{
public:
/**
* @brief A Navigator Muxer constructor
*/
56 NavigatorMuxer( )
: current_navigator_( std::string( "" ) ) {}
/**
* @brief Get the navigator muxer state
* @return bool If a navigator is in progress
*/
63 bool isNavigating( )
{
std::scoped_lock l( mutex_ );
return !current_navigator_.empty( );
}
/**
* @brief Start navigating with a given navigator
* @param string Name of the navigator to start
*/
73 void startNavigating( const std::string & navigator_name )
{
std::scoped_lock l( mutex_ );
if ( !current_navigator_.empty( ) ) {
RCLCPP_ERROR(
rclcpp::get_logger( "NavigatorMutex" ),
"Major error! Navigation requested while another navigation"
" task is in progress! This likely occurred from an incorrect"
"implementation of a navigator plugin." );
}
current_navigator_ = navigator_name;
}
/**
* @brief Stop navigating with a given navigator
* @param string Name of the navigator ending task
*/
90 void stopNavigating( const std::string & navigator_name )
{
std::scoped_lock l( mutex_ );
if ( current_navigator_ != navigator_name ) {
RCLCPP_ERROR(
rclcpp::get_logger( "NavigatorMutex" ),
"Major error! Navigation stopped while another navigation"
" task is in progress! This likely occurred from an incorrect"
"implementation of a navigator plugin." );
} else {
current_navigator_ = std::string( "" );
}
}
protected:
105 std::string current_navigator_;
106 std::mutex mutex_;
};
/**
* @class Navigator
* @brief Navigator interface that acts as a base class for all BT-based Navigator action's plugins
*/
template<class ActionT>
114 class Navigator
{
public:
using Ptr = std::shared_ptr<nav2_bt_navigator::Navigator<ActionT>>;
/**
* @brief A Navigator constructor
*/
Navigator( )
{
plugin_muxer_ = nullptr;
}
/**
* @brief Virtual destructor
*/
virtual ~Navigator( ) = default;
/**
* @brief Configuration to setup the navigator's backend BT and actions
* @param parent_node The ROS parent node to utilize
* @param plugin_lib_names a vector of plugin shared libraries to load
* @param feedback_utils Some utilities useful for navigators to have
* @param plugin_muxer The muxing object to ensure only one navigator
* can be active at a time
* @param odom_smoother Object to get current smoothed robot's speed
* @return bool If successful
*/
142 bool on_configure(
rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node,
const std::vector<std::string> & plugin_lib_names,
const FeedbackUtils & feedback_utils,
nav2_bt_navigator::NavigatorMuxer * plugin_muxer,
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother )
{
auto node = parent_node.lock( );
logger_ = node->get_logger( );
clock_ = node->get_clock( );
feedback_utils_ = feedback_utils;
plugin_muxer_ = plugin_muxer;
// get the default behavior tree for this navigator
std::string default_bt_xml_filename = getDefaultBTFilepath( parent_node );
// Create the Behavior Tree Action Server for this navigator
bt_action_server_ = std::make_unique<nav2_behavior_tree::BtActionServer<ActionT>>(
node,
getName( ),
plugin_lib_names,
default_bt_xml_filename,
std::bind( &Navigator::onGoalReceived, this, std::placeholders::_1 ),
std::bind( &Navigator::onLoop, this ),
std::bind( &Navigator::onPreempt, this, std::placeholders::_1 ),
std::bind( &Navigator::onCompletion, this, std::placeholders::_1, std::placeholders::_2 ) );
bool ok = true;
if ( !bt_action_server_->on_configure( ) ) {
ok = false;
}
BT::Blackboard::Ptr blackboard = bt_action_server_->getBlackboard( );
blackboard->set<std::shared_ptr<tf2_ros::Buffer>>( "tf_buffer", feedback_utils.tf ); // NOLINT
blackboard->set<bool>( "initial_pose_received", false ); // NOLINT
blackboard->set<int>( "number_recoveries", 0 ); // NOLINT
return configure( parent_node, odom_smoother ) && ok;
}
/**
* @brief Activation of the navigator's backend BT and actions
* @return bool If successful
*/
186 bool on_activate( )
{
bool ok = true;
if ( !bt_action_server_->on_activate( ) ) {
ok = false;
}
return activate( ) && ok;
}
/**
* @brief Deactivation of the navigator's backend BT and actions
* @return bool If successful
*/
201 bool on_deactivate( )
{
bool ok = true;
if ( !bt_action_server_->on_deactivate( ) ) {
ok = false;
}
return deactivate( ) && ok;
}
/**
* @brief Cleanup a navigator
* @return bool If successful
*/
215 bool on_cleanup( )
{
bool ok = true;
if ( !bt_action_server_->on_cleanup( ) ) {
ok = false;
}
bt_action_server_.reset( );
return cleanup( ) && ok;
}
/**
* @brief Get the action name of this navigator to expose
* @return string Name of action to expose
*/
231 virtual std::string getName( ) = 0;
virtual std::string getDefaultBTFilepath( rclcpp_lifecycle::LifecycleNode::WeakPtr node ) = 0;
/**
* @brief Get the action server
* @return Action server pointer
*/
std::unique_ptr<nav2_behavior_tree::BtActionServer<ActionT>> & getActionServer( )
{
return bt_action_server_;
}
protected:
/**
* @brief An intermediate goal reception function to mux navigators.
*/
bool onGoalReceived( typename ActionT::Goal::ConstSharedPtr goal )
{
if ( plugin_muxer_->isNavigating( ) ) {
RCLCPP_ERROR(
logger_,
"Requested navigation from %s while another navigator is processing, "
" rejecting request.", getName( ).c_str( ) );
return false;
}
bool goal_accepted = goalReceived( goal );
if ( goal_accepted ) {
plugin_muxer_->startNavigating( getName( ) );
}
return goal_accepted;
}
/**
* @brief An intermediate completion function to mux navigators
*/
void onCompletion(
typename ActionT::Result::SharedPtr result,
const nav2_behavior_tree::BtStatus final_bt_status )
{
plugin_muxer_->stopNavigating( getName( ) );
goalCompleted( result, final_bt_status );
}
/**
* @brief A callback to be called when a new goal is received by the BT action server
* Can be used to check if goal is valid and put values on
* the blackboard which depend on the received goal
*/
virtual bool goalReceived( typename ActionT::Goal::ConstSharedPtr goal ) = 0;
/**
* @brief A callback that defines execution that happens on one iteration through the BT
* Can be used to publish action feedback
*/
virtual void onLoop( ) = 0;
/**
* @brief A callback that is called when a preempt is requested
*/
virtual void onPreempt( typename ActionT::Goal::ConstSharedPtr goal ) = 0;
/**
* @brief A callback that is called when a the action is completed; Can fill in
* action result message or indicate that this action is done.
*/
virtual void goalCompleted(
typename ActionT::Result::SharedPtr result,
const nav2_behavior_tree::BtStatus final_bt_status ) = 0;
/**
* @param Method to configure resources.
*/
virtual bool configure(
rclcpp_lifecycle::LifecycleNode::WeakPtr /*node*/,
std::shared_ptr<nav2_util::OdomSmoother>/*odom_smoother*/ )
{
return true;
}
/**
* @brief Method to cleanup resources.
*/
virtual bool cleanup( ) {return true;}
/**
* @brief Method to activate any threads involved in execution.
*/
virtual bool activate( ) {return true;}
/**
* @brief Method to deactivate and any threads involved in execution.
*/
virtual bool deactivate( ) {return true;}
std::unique_ptr<nav2_behavior_tree::BtActionServer<ActionT>> bt_action_server_;
rclcpp::Logger logger_{rclcpp::get_logger( "Navigator" )};
rclcpp::Clock::SharedPtr clock_;
FeedbackUtils feedback_utils_;
NavigatorMuxer * plugin_muxer_;
};
} // namespace nav2_bt_navigator
#endif // NAV2_BT_NAVIGATOR__NAVIGATOR_HPP_
// Copyright ( c ) 2021 Samsung Research
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_THROUGH_POSES_HPP_
#define NAV2_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_THROUGH_POSES_HPP_
#include <string>
#include <vector>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_bt_navigator/navigator.hpp"
#include "nav2_msgs/action/navigate_through_poses.hpp"
#include "nav_msgs/msg/path.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_util/odometry_utils.hpp"
namespace nav2_bt_navigator
{
/**
* @class NavigateToPoseNavigator
* @brief A navigator for navigating to a a bunch of intermediary poses
*/
38 class NavigateThroughPosesNavigator
39 : public nav2_bt_navigator::Navigator<nav2_msgs::action::NavigateThroughPoses>
{
public:
using ActionT = nav2_msgs::action::NavigateThroughPoses;
typedef std::vector<geometry_msgs::msg::PoseStamped> Goals;
/**
* @brief A constructor for NavigateThroughPosesNavigator
*/
NavigateThroughPosesNavigator( )
: Navigator( ) {}
/**
* @brief A configure state transition to configure navigator's state
* @param node Weakptr to the lifecycle node
* @param odom_smoother Object to get current smoothed robot's speed
*/
bool configure(
rclcpp_lifecycle::LifecycleNode::WeakPtr node,
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother ) override;
/**
* @brief Get action name for this navigator
* @return string Name of action server
*/
64 std::string getName( ) override {return std::string( "navigate_through_poses" );}
/**
* @brief Get navigator's default BT
* @param node WeakPtr to the lifecycle node
* @return string Filepath to default XML
*/
71 std::string getDefaultBTFilepath( rclcpp_lifecycle::LifecycleNode::WeakPtr node ) override;
protected:
/**
* @brief A callback to be called when a new goal is received by the BT action server
* Can be used to check if goal is valid and put values on
* the blackboard which depend on the received goal
* @param goal Action template's goal message
* @return bool if goal was received successfully to be processed
*/
81 bool goalReceived( ActionT::Goal::ConstSharedPtr goal ) override;
/**
* @brief A callback that defines execution that happens on one iteration through the BT
* Can be used to publish action feedback
*/
87 void onLoop( ) override;
/**
* @brief A callback that is called when a preempt is requested
*/
92 void onPreempt( ActionT::Goal::ConstSharedPtr goal ) override;
/**
* @brief A callback that is called when a the action is completed, can fill in
* action result message or indicate that this action is done.
* @param result Action template result message to populate
* @param final_bt_status Resulting status of the behavior tree execution that may be
* referenced while populating the result.
*/
101 void goalCompleted(
typename ActionT::Result::SharedPtr result,
const nav2_behavior_tree::BtStatus final_bt_status ) override;
/**
* @brief Goal pose initialization on the blackboard
*/
108 void initializeGoalPoses( ActionT::Goal::ConstSharedPtr goal );
rclcpp::Time start_time_;
std::string goals_blackboard_id_;
std::string path_blackboard_id_;
// Odometry smoother object
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
};
} // namespace nav2_bt_navigator
#endif // NAV2_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_THROUGH_POSES_HPP_
// Copyright ( c ) 2021 Samsung Research
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_TO_POSE_HPP_
#define NAV2_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_TO_POSE_HPP_
#include <string>
#include <vector>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_bt_navigator/navigator.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav_msgs/msg/path.hpp"
#include "nav2_util/odometry_utils.hpp"
namespace nav2_bt_navigator
{
/**
* @class NavigateToPoseNavigator
* @brief A navigator for navigating to a specified pose
*/
38 class NavigateToPoseNavigator
39 : public nav2_bt_navigator::Navigator<nav2_msgs::action::NavigateToPose>
{
public:
using ActionT = nav2_msgs::action::NavigateToPose;
/**
* @brief A constructor for NavigateToPoseNavigator
*/
NavigateToPoseNavigator( )
: Navigator( ) {}
/**
* @brief A configure state transition to configure navigator's state
* @param node Weakptr to the lifecycle node
* @param odom_smoother Object to get current smoothed robot's speed
*/
bool configure(
rclcpp_lifecycle::LifecycleNode::WeakPtr node,
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother ) override;
/**
* @brief A cleanup state transition to remove memory allocated
*/
62 bool cleanup( ) override;
/**
* @brief A subscription and callback to handle the topic-based goal published
* from rviz
* @param pose Pose received via atopic
*/
69 void onGoalPoseReceived( const geometry_msgs::msg::PoseStamped::SharedPtr pose );
/**
* @brief Get action name for this navigator
* @return string Name of action server
*/
75 std::string getName( ) {return std::string( "navigate_to_pose" );}
/**
* @brief Get navigator's default BT
* @param node WeakPtr to the lifecycle node
* @return string Filepath to default XML
*/
82 std::string getDefaultBTFilepath( rclcpp_lifecycle::LifecycleNode::WeakPtr node ) override;
protected:
/**
* @brief A callback to be called when a new goal is received by the BT action server
* Can be used to check if goal is valid and put values on
* the blackboard which depend on the received goal
* @param goal Action template's goal message
* @return bool if goal was received successfully to be processed
*/
92 bool goalReceived( ActionT::Goal::ConstSharedPtr goal ) override;
/**
* @brief A callback that defines execution that happens on one iteration through the BT
* Can be used to publish action feedback
*/
98 void onLoop( ) override;
/**
* @brief A callback that is called when a preempt is requested
*/
103 void onPreempt( ActionT::Goal::ConstSharedPtr goal ) override;
/**
* @brief A callback that is called when a the action is completed, can fill in
* action result message or indicate that this action is done.
* @param result Action template result message to populate
* @param final_bt_status Resulting status of the behavior tree execution that may be
* referenced while populating the result.
*/
112 void goalCompleted(
typename ActionT::Result::SharedPtr result,
const nav2_behavior_tree::BtStatus final_bt_status ) override;
/**
* @brief Goal pose initialization on the blackboard
* @param goal Action template's goal message to process
*/
120 void initializeGoalPose( ActionT::Goal::ConstSharedPtr goal );
rclcpp::Time start_time_;
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_sub_;
rclcpp_action::Client<ActionT>::SharedPtr self_client_;
std::string goal_blackboard_id_;
std::string path_blackboard_id_;
// Odometry smoother object
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
};
} // namespace nav2_bt_navigator
#endif // NAV2_BT_NAVIGATOR__NAVIGATORS__NAVIGATE_TO_POSE_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "nav2_bt_navigator/bt_navigator.hpp"
#include <memory>
#include <string>
#include <utility>
#include <set>
#include <limits>
#include <vector>
#include "nav2_util/geometry_utils.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav2_behavior_tree/bt_conversions.hpp"
namespace nav2_bt_navigator
{
31 BtNavigator::BtNavigator( const rclcpp::NodeOptions & options )
: nav2_util::LifecycleNode( "bt_navigator", "", options )
{
RCLCPP_INFO( get_logger( ), "Creating" );
const std::vector<std::string> plugin_libs = {
"nav2_compute_path_to_pose_action_bt_node",
"nav2_compute_path_through_poses_action_bt_node",
"nav2_smooth_path_action_bt_node",
"nav2_follow_path_action_bt_node",
"nav2_spin_action_bt_node",
"nav2_wait_action_bt_node",
"nav2_back_up_action_bt_node",
"nav2_drive_on_heading_bt_node",
"nav2_clear_costmap_service_bt_node",
"nav2_is_stuck_condition_bt_node",
"nav2_goal_reached_condition_bt_node",
"nav2_initial_pose_received_condition_bt_node",
"nav2_goal_updated_condition_bt_node",
"nav2_globally_updated_goal_condition_bt_node",
"nav2_is_path_valid_condition_bt_node",
"nav2_reinitialize_global_localization_service_bt_node",
"nav2_rate_controller_bt_node",
"nav2_distance_controller_bt_node",
"nav2_speed_controller_bt_node",
"nav2_truncate_path_action_bt_node",
"nav2_truncate_path_local_action_bt_node",
"nav2_goal_updater_node_bt_node",
"nav2_recovery_node_bt_node",
"nav2_pipeline_sequence_bt_node",
"nav2_round_robin_node_bt_node",
"nav2_transform_available_condition_bt_node",
"nav2_time_expired_condition_bt_node",
"nav2_path_expiring_timer_condition",
"nav2_distance_traveled_condition_bt_node",
"nav2_single_trigger_bt_node",
"nav2_goal_updated_controller_bt_node",
"nav2_is_battery_low_condition_bt_node",
"nav2_navigate_through_poses_action_bt_node",
"nav2_navigate_to_pose_action_bt_node",
"nav2_remove_passed_goals_action_bt_node",
"nav2_planner_selector_bt_node",
"nav2_controller_selector_bt_node",
"nav2_goal_checker_selector_bt_node",
"nav2_controller_cancel_bt_node",
"nav2_path_longer_on_approach_bt_node"
"nav2_wait_cancel_bt_node",
"nav2_spin_cancel_bt_node",
"nav2_back_up_cancel_bt_node"
"nav2_drive_on_heading_cancel_bt_node"
};
declare_parameter( "plugin_lib_names", plugin_libs );
declare_parameter( "transform_tolerance", rclcpp::ParameterValue( 0.1 ) );
declare_parameter( "global_frame", std::string( "map" ) );
declare_parameter( "robot_base_frame", std::string( "base_link" ) );
declare_parameter( "odom_topic", std::string( "odom" ) );
}
90 BtNavigator::~BtNavigator( )
{
}
nav2_util::CallbackReturn
95 BtNavigator::on_configure( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Configuring" );
tf_ = std::make_shared<tf2_ros::Buffer>( get_clock( ) );
auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
get_node_base_interface( ), get_node_timers_interface( ) );
tf_->setCreateTimerInterface( timer_interface );
tf_->setUsingDedicatedThread( true );
tf_listener_ = std::make_shared<tf2_ros::TransformListener>( *tf_, this, false );
global_frame_ = get_parameter( "global_frame" ).as_string( );
robot_frame_ = get_parameter( "robot_base_frame" ).as_string( );
transform_tolerance_ = get_parameter( "transform_tolerance" ).as_double( );
odom_topic_ = get_parameter( "odom_topic" ).as_string( );
// Libraries to pull plugins ( BT Nodes ) from
auto plugin_lib_names = get_parameter( "plugin_lib_names" ).as_string_array( );
pose_navigator_ = std::make_unique<nav2_bt_navigator::NavigateToPoseNavigator>( );
poses_navigator_ = std::make_unique<nav2_bt_navigator::NavigateThroughPosesNavigator>( );
nav2_bt_navigator::FeedbackUtils feedback_utils;
feedback_utils.tf = tf_;
feedback_utils.global_frame = global_frame_;
feedback_utils.robot_frame = robot_frame_;
feedback_utils.transform_tolerance = transform_tolerance_;
// Odometry smoother object for getting current speed
odom_smoother_ = std::make_shared<nav2_util::OdomSmoother>( shared_from_this( ), 0.3, odom_topic_ );
if ( !pose_navigator_->on_configure(
shared_from_this( ), plugin_lib_names, feedback_utils, &plugin_muxer_, odom_smoother_ ) )
{
return nav2_util::CallbackReturn::FAILURE;
}
if ( !poses_navigator_->on_configure(
shared_from_this( ), plugin_lib_names, feedback_utils, &plugin_muxer_, odom_smoother_ ) )
{
return nav2_util::CallbackReturn::FAILURE;
}
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
142 BtNavigator::on_activate( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Activating" );
if ( !poses_navigator_->on_activate( ) || !pose_navigator_->on_activate( ) ) {
return nav2_util::CallbackReturn::FAILURE;
}
// create bond connection
createBond( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
157 BtNavigator::on_deactivate( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Deactivating" );
if ( !poses_navigator_->on_deactivate( ) || !pose_navigator_->on_deactivate( ) ) {
return nav2_util::CallbackReturn::FAILURE;
}
// destroy bond connection
destroyBond( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
172 BtNavigator::on_cleanup( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Cleaning up" );
// Reset the listener before the buffer
tf_listener_.reset( );
tf_.reset( );
if ( !poses_navigator_->on_cleanup( ) || !pose_navigator_->on_cleanup( ) ) {
return nav2_util::CallbackReturn::FAILURE;
}
poses_navigator_.reset( );
pose_navigator_.reset( );
RCLCPP_INFO( get_logger( ), "Completed Cleaning up" );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
192 BtNavigator::on_shutdown( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Shutting down" );
return nav2_util::CallbackReturn::SUCCESS;
}
} // namespace nav2_bt_navigator
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
205 RCLCPP_COMPONENTS_REGISTER_NODE( nav2_bt_navigator::BtNavigator )
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include "nav2_bt_navigator/bt_navigator.hpp"
#include "rclcpp/rclcpp.hpp"
20 int main( int argc, char ** argv )
{
rclcpp::init( argc, argv );
auto node = std::make_shared<nav2_bt_navigator::BtNavigator>( );
rclcpp::spin( node->get_node_base_interface( ) );
rclcpp::shutdown( );
return 0;
}
1 // Copyright ( c ) 2021 Samsung Research
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <vector>
#include <string>
#include <set>
#include <memory>
#include <limits>
#include "nav2_bt_navigator/navigators/navigate_through_poses.hpp"
namespace nav2_bt_navigator
{
bool
26 NavigateThroughPosesNavigator::configure(
27 rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node,
28 std::shared_ptr<nav2_util::OdomSmoother> odom_smoother )
{
start_time_ = rclcpp::Time( 0 );
auto node = parent_node.lock( );
if ( !node->has_parameter( "goals_blackboard_id" ) ) {
node->declare_parameter( "goals_blackboard_id", std::string( "goals" ) );
}
goals_blackboard_id_ = node->get_parameter( "goals_blackboard_id" ).as_string( );
if ( !node->has_parameter( "path_blackboard_id" ) ) {
node->declare_parameter( "path_blackboard_id", std::string( "path" ) );
}
path_blackboard_id_ = node->get_parameter( "path_blackboard_id" ).as_string( );
// Odometry smoother object for getting current speed
odom_smoother_ = odom_smoother;
return true;
}
std::string
52 NavigateThroughPosesNavigator::getDefaultBTFilepath(
53 rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node )
{
std::string default_bt_xml_filename;
auto node = parent_node.lock( );
if ( !node->has_parameter( "default_nav_through_poses_bt_xml" ) ) {
std::string pkg_share_dir =
ament_index_cpp::get_package_share_directory( "nav2_bt_navigator" );
node->declare_parameter<std::string>(
"default_nav_through_poses_bt_xml",
pkg_share_dir +
"/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml" );
}
node->get_parameter( "default_nav_through_poses_bt_xml", default_bt_xml_filename );
return default_bt_xml_filename;
}
bool
73 NavigateThroughPosesNavigator::goalReceived( ActionT::Goal::ConstSharedPtr goal )
{
auto bt_xml_filename = goal->behavior_tree;
if ( !bt_action_server_->loadBehaviorTree( bt_xml_filename ) ) {
RCLCPP_ERROR(
logger_, "Error loading XML file: %s. Navigation canceled.",
bt_xml_filename.c_str( ) );
return false;
}
initializeGoalPoses( goal );
return true;
}
void
90 NavigateThroughPosesNavigator::goalCompleted(
typename ActionT::Result::SharedPtr /*result*/,
const nav2_behavior_tree::BtStatus /*final_bt_status*/ )
{
}
void
97 NavigateThroughPosesNavigator::onLoop( )
{
using namespace nav2_util::geometry_utils; // NOLINT
// action server feedback ( pose, duration of task,
// number of recoveries, and distance remaining to goal, etc )
auto feedback_msg = std::make_shared<ActionT::Feedback>( );
auto blackboard = bt_action_server_->getBlackboard( );
Goals goal_poses;
blackboard->get<Goals>( goals_blackboard_id_, goal_poses );
if ( goal_poses.size( ) == 0 ) {
bt_action_server_->publishFeedback( feedback_msg );
return;
}
geometry_msgs::msg::PoseStamped current_pose;
nav2_util::getCurrentPose(
current_pose, *feedback_utils_.tf,
feedback_utils_.global_frame, feedback_utils_.robot_frame,
feedback_utils_.transform_tolerance );
try {
// Get current path points
nav_msgs::msg::Path current_path;
blackboard->get<nav_msgs::msg::Path>( path_blackboard_id_, current_path );
// Find the closest pose to current pose on global path
auto find_closest_pose_idx =
[¤t_pose, ¤t_path]( ) {
size_t closest_pose_idx = 0;
double curr_min_dist = std::numeric_limits<double>::max( );
for ( size_t curr_idx = 0; curr_idx < current_path.poses.size( ); ++curr_idx ) {
double curr_dist = nav2_util::geometry_utils::euclidean_distance(
current_pose, current_path.poses[curr_idx] );
if ( curr_dist < curr_min_dist ) {
curr_min_dist = curr_dist;
closest_pose_idx = curr_idx;
}
}
return closest_pose_idx;
};
// Calculate distance on the path
double distance_remaining =
nav2_util::geometry_utils::calculate_path_length( current_path, find_closest_pose_idx( ) );
// Default value for time remaining
rclcpp::Duration estimated_time_remaining = rclcpp::Duration::from_seconds( 0.0 );
// Get current speed
geometry_msgs::msg::Twist current_odom = odom_smoother_->getTwist( );
double current_linear_speed = std::hypot( current_odom.linear.x, current_odom.linear.y );
// Calculate estimated time taken to goal if speed is higher than 1cm/s
// and at least 10cm to go
if ( ( std::abs( current_linear_speed ) > 0.01 ) && ( distance_remaining > 0.1 ) ) {
estimated_time_remaining =
rclcpp::Duration::from_seconds( distance_remaining / std::abs( current_linear_speed ) );
}
feedback_msg->distance_remaining = distance_remaining;
feedback_msg->estimated_time_remaining = estimated_time_remaining;
} catch ( ... ) {
// Ignore
}
int recovery_count = 0;
blackboard->get<int>( "number_recoveries", recovery_count );
feedback_msg->number_of_recoveries = recovery_count;
feedback_msg->current_pose = current_pose;
feedback_msg->navigation_time = clock_->now( ) - start_time_;
feedback_msg->number_of_poses_remaining = goal_poses.size( );
bt_action_server_->publishFeedback( feedback_msg );
}
void
177 NavigateThroughPosesNavigator::onPreempt( ActionT::Goal::ConstSharedPtr goal )
{
RCLCPP_INFO( logger_, "Received goal preemption request" );
if ( goal->behavior_tree == bt_action_server_->getCurrentBTFilename( ) ||
( goal->behavior_tree.empty( ) &&
bt_action_server_->getCurrentBTFilename( ) == bt_action_server_->getDefaultBTFilename( ) ) )
{
// if pending goal requests the same BT as the current goal, accept the pending goal
// if pending goal has an empty behavior_tree field, it requests the default BT file
// accept the pending goal if the current goal is running the default BT file
initializeGoalPoses( bt_action_server_->acceptPendingGoal( ) );
} else {
RCLCPP_WARN(
logger_,
"Preemption request was rejected since the requested BT XML file is not the same "
"as the one that the current goal is executing. Preemption with a new BT is invalid "
"since it would require cancellation of the previous goal instead of true preemption."
"\nCancel the current goal and send a new action request if you want to use a "
"different BT XML file. For now, continuing to track the last goal until completion." );
bt_action_server_->terminatePendingGoal( );
}
}
void
202 NavigateThroughPosesNavigator::initializeGoalPoses( ActionT::Goal::ConstSharedPtr goal )
{
if ( goal->poses.size( ) > 0 ) {
RCLCPP_INFO(
logger_, "Begin navigating from current location through %zu poses to ( %.2f, %.2f )",
goal->poses.size( ), goal->poses.back( ).pose.position.x, goal->poses.back( ).pose.position.y );
}
// Reset state for new action feedback
start_time_ = clock_->now( );
auto blackboard = bt_action_server_->getBlackboard( );
blackboard->set<int>( "number_recoveries", 0 ); // NOLINT
// Update the goal pose on the blackboard
blackboard->set<Goals>( goals_blackboard_id_, goal->poses );
}
} // namespace nav2_bt_navigator
1 // Copyright ( c ) 2021 Samsung Research
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <vector>
#include <string>
#include <set>
#include <memory>
#include <limits>
#include "nav2_bt_navigator/navigators/navigate_to_pose.hpp"
namespace nav2_bt_navigator
{
bool
26 NavigateToPoseNavigator::configure(
27 rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node,
28 std::shared_ptr<nav2_util::OdomSmoother> odom_smoother )
{
start_time_ = rclcpp::Time( 0 );
auto node = parent_node.lock( );
if ( !node->has_parameter( "goal_blackboard_id" ) ) {
node->declare_parameter( "goal_blackboard_id", std::string( "goal" ) );
}
goal_blackboard_id_ = node->get_parameter( "goal_blackboard_id" ).as_string( );
if ( !node->has_parameter( "path_blackboard_id" ) ) {
node->declare_parameter( "path_blackboard_id", std::string( "path" ) );
}
path_blackboard_id_ = node->get_parameter( "path_blackboard_id" ).as_string( );
// Odometry smoother object for getting current speed
odom_smoother_ = odom_smoother;
self_client_ = rclcpp_action::create_client<ActionT>( node, getName( ) );
goal_sub_ = node->create_subscription<geometry_msgs::msg::PoseStamped>(
"goal_pose",
rclcpp::SystemDefaultsQoS( ),
std::bind( &NavigateToPoseNavigator::onGoalPoseReceived, this, std::placeholders::_1 ) );
return true;
}
std::string
58 NavigateToPoseNavigator::getDefaultBTFilepath(
59 rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node )
{
std::string default_bt_xml_filename;
auto node = parent_node.lock( );
if ( !node->has_parameter( "default_nav_to_pose_bt_xml" ) ) {
std::string pkg_share_dir =
ament_index_cpp::get_package_share_directory( "nav2_bt_navigator" );
node->declare_parameter<std::string>(
"default_nav_to_pose_bt_xml",
pkg_share_dir +
"/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml" );
}
node->get_parameter( "default_nav_to_pose_bt_xml", default_bt_xml_filename );
return default_bt_xml_filename;
}
bool
79 NavigateToPoseNavigator::cleanup( )
{
goal_sub_.reset( );
self_client_.reset( );
return true;
}
bool
87 NavigateToPoseNavigator::goalReceived( ActionT::Goal::ConstSharedPtr goal )
{
auto bt_xml_filename = goal->behavior_tree;
if ( !bt_action_server_->loadBehaviorTree( bt_xml_filename ) ) {
RCLCPP_ERROR(
logger_, "BT file not found: %s. Navigation canceled.",
bt_xml_filename.c_str( ) );
return false;
}
initializeGoalPose( goal );
return true;
}
void
104 NavigateToPoseNavigator::goalCompleted(
typename ActionT::Result::SharedPtr /*result*/,
const nav2_behavior_tree::BtStatus /*final_bt_status*/ )
{
}
void
111 NavigateToPoseNavigator::onLoop( )
{
// action server feedback ( pose, duration of task,
// number of recoveries, and distance remaining to goal )
auto feedback_msg = std::make_shared<ActionT::Feedback>( );
geometry_msgs::msg::PoseStamped current_pose;
nav2_util::getCurrentPose(
current_pose, *feedback_utils_.tf,
feedback_utils_.global_frame, feedback_utils_.robot_frame,
feedback_utils_.transform_tolerance );
auto blackboard = bt_action_server_->getBlackboard( );
try {
// Get current path points
nav_msgs::msg::Path current_path;
blackboard->get<nav_msgs::msg::Path>( path_blackboard_id_, current_path );
// Find the closest pose to current pose on global path
auto find_closest_pose_idx =
[¤t_pose, ¤t_path]( ) {
size_t closest_pose_idx = 0;
double curr_min_dist = std::numeric_limits<double>::max( );
for ( size_t curr_idx = 0; curr_idx < current_path.poses.size( ); ++curr_idx ) {
double curr_dist = nav2_util::geometry_utils::euclidean_distance(
current_pose, current_path.poses[curr_idx] );
if ( curr_dist < curr_min_dist ) {
curr_min_dist = curr_dist;
closest_pose_idx = curr_idx;
}
}
return closest_pose_idx;
};
// Calculate distance on the path
double distance_remaining =
nav2_util::geometry_utils::calculate_path_length( current_path, find_closest_pose_idx( ) );
// Default value for time remaining
rclcpp::Duration estimated_time_remaining = rclcpp::Duration::from_seconds( 0.0 );
// Get current speed
geometry_msgs::msg::Twist current_odom = odom_smoother_->getTwist( );
double current_linear_speed = std::hypot( current_odom.linear.x, current_odom.linear.y );
// Calculate estimated time taken to goal if speed is higher than 1cm/s
// and at least 10cm to go
if ( ( std::abs( current_linear_speed ) > 0.01 ) && ( distance_remaining > 0.1 ) ) {
estimated_time_remaining =
rclcpp::Duration::from_seconds( distance_remaining / std::abs( current_linear_speed ) );
}
feedback_msg->distance_remaining = distance_remaining;
feedback_msg->estimated_time_remaining = estimated_time_remaining;
} catch ( ... ) {
// Ignore
}
int recovery_count = 0;
blackboard->get<int>( "number_recoveries", recovery_count );
feedback_msg->number_of_recoveries = recovery_count;
feedback_msg->current_pose = current_pose;
feedback_msg->navigation_time = clock_->now( ) - start_time_;
bt_action_server_->publishFeedback( feedback_msg );
}
void
180 NavigateToPoseNavigator::onPreempt( ActionT::Goal::ConstSharedPtr goal )
{
RCLCPP_INFO( logger_, "Received goal preemption request" );
if ( goal->behavior_tree == bt_action_server_->getCurrentBTFilename( ) ||
( goal->behavior_tree.empty( ) &&
bt_action_server_->getCurrentBTFilename( ) == bt_action_server_->getDefaultBTFilename( ) ) )
{
// if pending goal requests the same BT as the current goal, accept the pending goal
// if pending goal has an empty behavior_tree field, it requests the default BT file
// accept the pending goal if the current goal is running the default BT file
initializeGoalPose( bt_action_server_->acceptPendingGoal( ) );
} else {
RCLCPP_WARN(
logger_,
"Preemption request was rejected since the requested BT XML file is not the same "
"as the one that the current goal is executing. Preemption with a new BT is invalid "
"since it would require cancellation of the previous goal instead of true preemption."
"\nCancel the current goal and send a new action request if you want to use a "
"different BT XML file. For now, continuing to track the last goal until completion." );
bt_action_server_->terminatePendingGoal( );
}
}
void
205 NavigateToPoseNavigator::initializeGoalPose( ActionT::Goal::ConstSharedPtr goal )
{
RCLCPP_INFO(
logger_, "Begin navigating from current location to ( %.2f, %.2f )",
goal->pose.pose.position.x, goal->pose.pose.position.y );
// Reset state for new action feedback
start_time_ = clock_->now( );
auto blackboard = bt_action_server_->getBlackboard( );
blackboard->set<int>( "number_recoveries", 0 ); // NOLINT
// Update the goal pose on the blackboard
blackboard->set<geometry_msgs::msg::PoseStamped>( goal_blackboard_id_, goal->pose );
}
void
221 NavigateToPoseNavigator::onGoalPoseReceived( const geometry_msgs::msg::PoseStamped::SharedPtr pose )
{
ActionT::Goal goal;
goal.pose = *pose;
self_client_->async_send_goal( goal );
}
} // namespace nav2_bt_navigator
// Copyright ( c ) 2021 RoboTech Vision
// Copyright ( c ) 2020 Shrijit Singh
// Copyright ( c ) 2020 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_CONSTRAINED_SMOOTHER__CONSTRAINED_SMOOTHER_HPP_
#define NAV2_CONSTRAINED_SMOOTHER__CONSTRAINED_SMOOTHER_HPP_
#include <string>
#include <vector>
#include <memory>
#include <algorithm>
#include "nav2_core/smoother.hpp"
#include "nav2_constrained_smoother/smoother.hpp"
#include "rclcpp/rclcpp.hpp"
#include "nav2_util/odometry_utils.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "geometry_msgs/msg/pose2_d.hpp"
namespace nav2_constrained_smoother
{
/**
* @class nav2_constrained_smoother::ConstrainedSmoother
* @brief Regulated pure pursuit controller plugin
*/
39 class ConstrainedSmoother : public nav2_core::Smoother
{
public:
/**
* @brief Constructor for nav2_constrained_smoother::ConstrainedSmoother
*/
ConstrainedSmoother( ) = default;
/**
* @brief Destrructor for nav2_constrained_smoother::ConstrainedSmoother
*/
~ConstrainedSmoother( ) override = default;
/**
* @brief Configure smoother parameters and member variables
* @param parent WeakPtr to node
* @param name Name of plugin
* @param tf TF buffer
* @param costmap_ros Costmap2DROS object of environment
*/
void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub,
std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub ) override;
/**
* @brief Cleanup controller state machine
*/
void cleanup( ) override;
/**
* @brief Activate controller state machine
*/
void activate( ) override;
/**
* @brief Deactivate controller state machine
*/
void deactivate( ) override;
/**
* @brief Method to smooth given path
*
* @param path In-out path to be optimized
* @param max_time Maximum duration smoothing should take
* @return Smoothed path
*/
bool smooth(
nav_msgs::msg::Path & path,
const rclcpp::Duration & max_time ) override;
protected:
std::shared_ptr<tf2_ros::Buffer> tf_;
std::string plugin_name_;
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
rclcpp::Logger logger_ {rclcpp::get_logger( "ConstrainedSmoother" )};
std::unique_ptr<nav2_constrained_smoother::Smoother> smoother_;
SmootherParams smoother_params_;
OptimizerParams optimizer_params_;
};
} // namespace nav2_constrained_smoother
#endif // NAV2_CONSTRAINED_SMOOTHER__CONSTRAINED_SMOOTHER_HPP_
1 // Copyright ( c ) 2021 RoboTech Vision
// Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef NAV2_CONSTRAINED_SMOOTHER__SMOOTHER_HPP_
#define NAV2_CONSTRAINED_SMOOTHER__SMOOTHER_HPP_
#include <cmath>
#include <vector>
#include <iostream>
#include <memory>
#include <queue>
#include <utility>
#include <deque>
#include <limits>
#include <algorithm>
#include "nav2_constrained_smoother/smoother_cost_function.hpp"
#include "nav2_constrained_smoother/utils.hpp"
#include "ceres/ceres.h"
#include "Eigen/Core"
namespace nav2_constrained_smoother
{
/**
* @class nav2_smac_planner::Smoother
* @brief A Conjugate Gradient 2D path smoother implementation
*/
42 class Smoother
{
public:
/**
* @brief A constructor for nav2_smac_planner::Smoother
*/
48 Smoother( ) {}
/**
* @brief A destructor for nav2_smac_planner::Smoother
*/
53 ~Smoother( ) {}
/**
* @brief Initialization of the smoother
* @param params OptimizerParam struct
*/
59 void initialize( const OptimizerParams params )
{
debug_ = params.debug;
options_.linear_solver_type = params.solver_types.at( params.linear_solver_type );
options_.max_num_iterations = params.max_iterations;
options_.function_tolerance = params.fn_tol;
options_.gradient_tolerance = params.gradient_tol;
options_.parameter_tolerance = params.param_tol;
if ( debug_ ) {
options_.minimizer_progress_to_stdout = true;
options_.logging_type = ceres::LoggingType::PER_MINIMIZER_ITERATION;
} else {
options_.logging_type = ceres::SILENT;
}
}
/**
* @brief Smoother method
* @param path Reference to path
* @param start_dir Orientation of the first pose
* @param end_dir Orientation of the last pose
* @param costmap Pointer to minimal costmap
* @param params parameters weights
* @return If smoothing was successful
*/
88 bool smooth(
89 std::vector<Eigen::Vector3d> & path,
90 const Eigen::Vector2d & start_dir,
91 const Eigen::Vector2d & end_dir,
92 const nav2_costmap_2d::Costmap2D * costmap,
const SmootherParams & params )
{
// Path has always at least 2 points
if ( path.size( ) < 2 ) {
throw std::runtime_error( "Constrained smoother: Path must have at least 2 points" );
}
options_.max_solver_time_in_seconds = params.max_time;
ceres::Problem problem;
std::vector<Eigen::Vector3d> path_optim;
std::vector<bool> optimized;
if ( buildProblem( path, costmap, params, problem, path_optim, optimized ) ) {
// solve the problem
ceres::Solver::Summary summary;
ceres::Solve( options_, &problem, &summary );
if ( debug_ ) {
RCLCPP_INFO( rclcpp::get_logger( "smoother_server" ), "%s", summary.FullReport( ).c_str( ) );
}
if ( !summary.IsSolutionUsable( ) || summary.initial_cost - summary.final_cost < 0.0 ) {
return false;
}
} else {
RCLCPP_INFO( rclcpp::get_logger( "smoother_server" ), "Path too short to optimize" );
}
upsampleAndPopulate( path_optim, optimized, start_dir, end_dir, params, path );
return true;
}
private:
/**
* @brief Build problem method
* @param path Reference to path
* @param costmap Pointer to costmap
* @param params Smoother parameters
* @param problem Output problem to solve
* @param path_optim Output path on which the problem will be solved
* @param optimized False for points skipped by downsampling
* @return If there is a problem to solve
*/
135 bool buildProblem(
136 const std::vector<Eigen::Vector3d> & path,
137 const nav2_costmap_2d::Costmap2D * costmap,
const SmootherParams & params,
139 ceres::Problem & problem,
140 std::vector<Eigen::Vector3d> & path_optim,
141 std::vector<bool> & optimized )
{
// Create costmap grid
costmap_grid_ = std::make_shared<ceres::Grid2D<u_char>>(
costmap->getCharMap( ), 0, costmap->getSizeInCellsY( ), 0, costmap->getSizeInCellsX( ) );
auto costmap_interpolator = std::make_shared<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>>(
*costmap_grid_ );
// Create residual blocks
const double cusp_half_length = params.cusp_zone_length / 2;
ceres::LossFunction * loss_function = NULL;
path_optim = path;
optimized = std::vector<bool>( path.size( ) );
optimized[0] = true;
int prelast_i = -1;
int last_i = 0;
double last_direction = path_optim[0][2];
bool last_was_cusp = false;
bool last_is_reversing = false;
std::deque<std::pair<double, SmootherCostFunction *>> potential_cusp_funcs;
double last_segment_len = EPSILON;
double potential_cusp_funcs_len = 0;
double len_since_cusp = std::numeric_limits<double>::infinity( );
for ( size_t i = 1; i < path_optim.size( ); i++ ) {
auto & pt = path_optim[i];
bool is_cusp = false;
if ( i != path_optim.size( ) - 1 ) {
is_cusp = pt[2] * last_direction < 0;
last_direction = pt[2];
// skip to downsample if can be skipped ( no forward/reverse direction change )
if ( !is_cusp &&
i > ( params.keep_start_orientation ? 1 : 0 ) &&
i < path_optim.size( ) - ( params.keep_goal_orientation ? 2 : 1 ) &&
static_cast<int>( i - last_i ) < params.path_downsampling_factor )
{
continue;
}
}
// keep distance inequalities between poses
// ( some might have been downsampled while others might not )
double current_segment_len = ( path_optim[i] - path_optim[last_i] ).block<2, 1>( 0, 0 ).norm( );
// forget cost functions which don't have chance to be part of a cusp zone
potential_cusp_funcs_len += current_segment_len;
while ( !potential_cusp_funcs.empty( ) && potential_cusp_funcs_len > cusp_half_length ) {
potential_cusp_funcs_len -= potential_cusp_funcs.front( ).first;
potential_cusp_funcs.pop_front( );
}
// update cusp zone costmap weights
if ( is_cusp ) {
double len_to_cusp = current_segment_len;
for ( int i = potential_cusp_funcs.size( ) - 1; i >= 0; i-- ) {
auto & f = potential_cusp_funcs[i];
double new_weight =
params.cusp_costmap_weight * ( 1.0 - len_to_cusp / cusp_half_length ) +
params.costmap_weight * len_to_cusp / cusp_half_length;
if ( std::abs( new_weight - params.cusp_costmap_weight ) <
std::abs( f.second->getCostmapWeight( ) - params.cusp_costmap_weight ) )
{
f.second->setCostmapWeight( new_weight );
}
len_to_cusp += f.first;
}
potential_cusp_funcs_len = 0;
potential_cusp_funcs.clear( );
len_since_cusp = 0;
}
// add cost function
optimized[i] = true;
if ( prelast_i != -1 ) {
double costmap_weight = params.costmap_weight;
if ( len_since_cusp <= cusp_half_length ) {
costmap_weight =
params.cusp_costmap_weight * ( 1.0 - len_since_cusp / cusp_half_length ) +
params.costmap_weight * len_since_cusp / cusp_half_length;
}
SmootherCostFunction * cost_function = new SmootherCostFunction(
path[last_i].template block<2, 1>(
0,
0 ),
( last_was_cusp ? -1 : 1 ) * last_segment_len / current_segment_len,
last_is_reversing,
costmap,
costmap_interpolator,
params,
costmap_weight
);
problem.AddResidualBlock(
cost_function->AutoDiff( ), loss_function,
path_optim[last_i].data( ), pt.data( ), path_optim[prelast_i].data( ) );
potential_cusp_funcs.emplace_back( current_segment_len, cost_function );
}
// shift current to last and last to pre-last
last_was_cusp = is_cusp;
last_is_reversing = last_direction < 0;
prelast_i = last_i;
last_i = i;
len_since_cusp += current_segment_len;
last_segment_len = std::max( EPSILON, current_segment_len );
}
int posesToOptimize = problem.NumParameterBlocks( ) - 2; // minus start and goal
if ( params.keep_goal_orientation ) {
posesToOptimize -= 1; // minus goal orientation holder
}
if ( params.keep_start_orientation ) {
posesToOptimize -= 1; // minus start orientation holder
}
if ( posesToOptimize <= 0 ) {
return false; // nothing to optimize
}
// first two and last two points are constant ( to keep start and end direction )
problem.SetParameterBlockConstant( path_optim.front( ).data( ) );
if ( params.keep_start_orientation ) {
problem.SetParameterBlockConstant( path_optim[1].data( ) );
}
if ( params.keep_goal_orientation ) {
problem.SetParameterBlockConstant( path_optim[path_optim.size( ) - 2].data( ) );
}
problem.SetParameterBlockConstant( path_optim.back( ).data( ) );
return true;
}
/**
* @brief Populate optimized points to path, assigning orientations and upsampling poses using cubic bezier
* @param path_optim Path with optimized points
* @param optimized False for points skipped by downsampling
* @param start_dir Orientation of the first pose
* @param end_dir Orientation of the last pose
* @param params Smoother parameters
* @param path Output path with upsampled optimized points
*/
280 void upsampleAndPopulate(
281 const std::vector<Eigen::Vector3d> & path_optim,
282 const std::vector<bool> & optimized,
283 const Eigen::Vector2d & start_dir,
284 const Eigen::Vector2d & end_dir,
const SmootherParams & params,
286 std::vector<Eigen::Vector3d> & path )
{
// Populate path, assign orientations, interpolate skipped/upsampled poses
path.clear( );
if ( params.path_upsampling_factor > 1 ) {
path.reserve( params.path_upsampling_factor * ( path_optim.size( ) - 1 ) + 1 );
}
int last_i = 0;
int prelast_i = -1;
Eigen::Vector2d prelast_dir;
for ( int i = 1; i <= static_cast<int>( path_optim.size( ) ); i++ ) {
if ( i == static_cast<int>( path_optim.size( ) ) || optimized[i] ) {
if ( prelast_i != -1 ) {
Eigen::Vector2d last_dir;
auto & prelast = path_optim[prelast_i];
auto & last = path_optim[last_i];
// Compute orientation of last
if ( i < static_cast<int>( path_optim.size( ) ) ) {
auto & current = path_optim[i];
Eigen::Vector2d tangent_dir = tangentDir<double>(
prelast.block<2, 1>( 0, 0 ),
last.block<2, 1>( 0, 0 ),
current.block<2, 1>( 0, 0 ),
prelast[2] * last[2] < 0 );
last_dir =
tangent_dir.dot( ( current - last ).block<2, 1>( 0, 0 ) * last[2] ) >= 0 ?
tangent_dir :
-tangent_dir;
last_dir.normalize( );
} else if ( params.keep_goal_orientation ) {
last_dir = end_dir;
} else {
last_dir = ( last - prelast ).block<2, 1>( 0, 0 ) * last[2];
last_dir.normalize( );
}
double last_angle = atan2( last_dir[1], last_dir[0] );
// Interpolate poses between prelast and last
int interp_cnt = ( last_i - prelast_i ) * params.path_upsampling_factor - 1;
if ( interp_cnt > 0 ) {
Eigen::Vector2d last_pt = last.block<2, 1>( 0, 0 );
Eigen::Vector2d prelast_pt = prelast.block<2, 1>( 0, 0 );
double dist = ( last_pt - prelast_pt ).norm( );
Eigen::Vector2d pt1 = prelast_pt + prelast_dir * dist * 0.4 * prelast[2];
Eigen::Vector2d pt2 = last_pt - last_dir * dist * 0.4 * prelast[2];
for ( int j = 1; j <= interp_cnt; j++ ) {
double interp = j / static_cast<double>( interp_cnt + 1 );
Eigen::Vector2d pt = cubicBezier( prelast_pt, pt1, pt2, last_pt, interp );
path.emplace_back( pt[0], pt[1], 0.0 );
}
}
path.emplace_back( last[0], last[1], last_angle );
// Assign orientations to interpolated points
for ( size_t j = path.size( ) - 1 - interp_cnt; j < path.size( ) - 1; j++ ) {
Eigen::Vector2d tangent_dir = tangentDir<double>(
path[j - 1].block<2, 1>( 0, 0 ),
path[j].block<2, 1>( 0, 0 ),
path[j + 1].block<2, 1>( 0, 0 ),
false );
tangent_dir =
tangent_dir.dot( ( path[j + 1] - path[j] ).block<2, 1>( 0, 0 ) * prelast[2] ) >= 0 ?
tangent_dir :
-tangent_dir;
path[j][2] = atan2( tangent_dir[1], tangent_dir[0] );
}
prelast_dir = last_dir;
} else { // start pose
auto & start = path_optim[0];
Eigen::Vector2d dir = params.keep_start_orientation ?
start_dir :
( ( path_optim[i] - start ).block<2, 1>( 0, 0 ) * start[2] ).normalized( );
path.emplace_back( start[0], start[1], atan2( dir[1], dir[0] ) );
prelast_dir = dir;
}
prelast_i = last_i;
last_i = i;
}
}
}
/*
Piecewise cubic bezier curve as defined by Adobe in Postscript
The two end points are pt0 and pt3
Their associated control points are pt1 and pt2
*/
375 static Eigen::Vector2d cubicBezier(
376 Eigen::Vector2d & pt0, Eigen::Vector2d & pt1,
377 Eigen::Vector2d & pt2, Eigen::Vector2d & pt3, double mu )
{
Eigen::Vector2d a, b, c, pt;
c[0] = 3 * ( pt1[0] - pt0[0] );
c[1] = 3 * ( pt1[1] - pt0[1] );
b[0] = 3 * ( pt2[0] - pt1[0] ) - c[0];
b[1] = 3 * ( pt2[1] - pt1[1] ) - c[1];
a[0] = pt3[0] - pt0[0] - c[0] - b[0];
a[1] = pt3[1] - pt0[1] - c[1] - b[1];
pt[0] = a[0] * mu * mu * mu + b[0] * mu * mu + c[0] * mu + pt0[0];
pt[1] = a[1] * mu * mu * mu + b[1] * mu * mu + c[1] * mu + pt0[1];
return pt;
}
394 bool debug_;
395 ceres::Solver::Options options_;
std::shared_ptr<ceres::Grid2D<u_char>> costmap_grid_;
};
} // namespace nav2_constrained_smoother
#endif // NAV2_CONSTRAINED_SMOOTHER__SMOOTHER_HPP_
// Copyright ( c ) 2021 RoboTech Vision
// Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef NAV2_CONSTRAINED_SMOOTHER__SMOOTHER_COST_FUNCTION_HPP_
#define NAV2_CONSTRAINED_SMOOTHER__SMOOTHER_COST_FUNCTION_HPP_
#include <cmath>
#include <vector>
#include <iostream>
#include <unordered_map>
#include <memory>
#include <queue>
#include <utility>
#include "ceres/ceres.h"
#include "ceres/cubic_interpolation.h"
#include "Eigen/Core"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_constrained_smoother/options.hpp"
#include "nav2_constrained_smoother/utils.hpp"
namespace nav2_constrained_smoother
{
/**
* @struct nav2_constrained_smoother::SmootherCostFunction
* @brief Cost function for path smoothing with multiple terms
* including curvature, smoothness, distance from original and obstacle avoidance.
*/
42 class SmootherCostFunction
{
public:
/**
* @brief A constructor for nav2_constrained_smoother::SmootherCostFunction
* @param original_path Original position of the path node
* @param next_to_last_length_ratio Ratio of next path segment compared to previous.
* Negative if one of them represents reversing motion.
* @param reversing Whether the path segment after this node represents reversing motion.
* @param costmap A costmap to get values for collision and obstacle avoidance
* @param params Optimization weights and parameters
* @param costmap_weight Costmap cost weight. Can be params.costmap_weight or params.cusp_costmap_weight
*/
55 SmootherCostFunction(
const Eigen::Vector2d & original_pos,
double next_to_last_length_ratio,
bool reversing,
const nav2_costmap_2d::Costmap2D * costmap,
const std::shared_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>> & costmap_interpolator,
const SmootherParams & params,
double costmap_weight )
: original_pos_( original_pos ),
next_to_last_length_ratio_( next_to_last_length_ratio ),
reversing_( reversing ),
params_( params ),
costmap_weight_( costmap_weight ),
costmap_origin_( costmap->getOriginX( ), costmap->getOriginY( ) ),
costmap_resolution_( costmap->getResolution( ) ),
costmap_interpolator_( costmap_interpolator )
{
}
74 ceres::CostFunction * AutoDiff( )
{
return new ceres::AutoDiffCostFunction<SmootherCostFunction, 4, 2, 2, 2>( this );
}
79 void setCostmapWeight( double costmap_weight )
{
costmap_weight_ = costmap_weight;
}
84 double getCostmapWeight( )
{
return costmap_weight_;
}
/**
* @brief Smoother cost function evaluation
* @param pt X, Y coords of current point
* @param pt_next X, Y coords of next point
* @param pt_prev X, Y coords of previous point
* @param pt_residual array of output residuals ( smoothing, curvature, distance, cost )
* @return if successful in computing values
*/
template<typename T>
bool operator( )(
const T * const pt, const T * const pt_next, const T * const pt_prev,
T * pt_residual ) const
{
Eigen::Map<const Eigen::Matrix<T, 2, 1>> xi( pt );
Eigen::Map<const Eigen::Matrix<T, 2, 1>> xi_next( pt_next );
Eigen::Map<const Eigen::Matrix<T, 2, 1>> xi_prev( pt_prev );
Eigen::Map<Eigen::Matrix<T, 4, 1>> residual( pt_residual );
residual.setZero( );
// compute cost
addSmoothingResidual<T>( params_.smooth_weight, xi, xi_next, xi_prev, residual[0] );
addCurvatureResidual<T>( params_.curvature_weight, xi, xi_next, xi_prev, residual[1] );
addDistanceResidual<T>(
params_.distance_weight, xi,
original_pos_.template cast<T>( ), residual[2] );
addCostResidual<T>( costmap_weight_, xi, xi_next, xi_prev, residual[3] );
return true;
}
protected:
/**
* @brief Cost function term for smooth paths
* @param weight Weight to apply to function
* @param pt Point Xi for evaluation
* @param pt_next Point Xi+1 for calculating Xi's cost
* @param pt_prev Point Xi-1 for calculating Xi's cost
* @param r Residual ( cost ) of term
*/
template<typename T>
inline void addSmoothingResidual(
const double & weight,
const Eigen::Matrix<T, 2, 1> & pt,
const Eigen::Matrix<T, 2, 1> & pt_next,
const Eigen::Matrix<T, 2, 1> & pt_prev,
T & r ) const
{
Eigen::Matrix<T, 2, 1> d_next = pt_next - pt;
Eigen::Matrix<T, 2, 1> d_prev = pt - pt_prev;
Eigen::Matrix<T, 2, 1> d_diff = next_to_last_length_ratio_ * d_next - d_prev;
139 r += ( T )weight * d_diff.dot( d_diff ); // objective function value
}
/**
* @brief Cost function term for maximum curved paths
* @param weight Weight to apply to function
* @param pt Point Xi for evaluation
* @param pt_next Point Xi+1 for calculating Xi's cost
* @param pt_prev Point Xi-1 for calculating Xi's cost
* @param curvature_params A struct to cache computations for the jacobian to use
* @param r Residual ( cost ) of term
*/
template<typename T>
inline void addCurvatureResidual(
const double & weight,
const Eigen::Matrix<T, 2, 1> & pt,
const Eigen::Matrix<T, 2, 1> & pt_next,
const Eigen::Matrix<T, 2, 1> & pt_prev,
T & r ) const
{
Eigen::Matrix<T, 2, 1> center = arcCenter(
pt_prev, pt, pt_next,
next_to_last_length_ratio_ < 0 );
if ( ceres::IsInfinite( center[0] ) ) {
return;
}
T turning_rad = ( pt - center ).norm( );
T ki_minus_kmax = ( T )1.0 / turning_rad - params_.max_curvature;
if ( ki_minus_kmax <= ( T )EPSILON ) {
return;
}
r += ( T )weight * ki_minus_kmax * ki_minus_kmax; // objective function value
}
/**
* @brief Cost function derivative term for steering away changes in pose
* @param weight Weight to apply to function
* @param xi Point Xi for evaluation
* @param xi_original original point Xi for evaluation
* @param r Residual ( cost ) of term
*/
template<typename T>
inline void addDistanceResidual(
const double & weight,
const Eigen::Matrix<T, 2, 1> & xi,
const Eigen::Matrix<T, 2, 1> & xi_original,
T & r ) const
{
r += ( T )weight * ( xi - xi_original ).squaredNorm( ); // objective function value
}
/**
* @brief Cost function term for steering away from costs
* @param weight Weight to apply to function
* @param value Point Xi's cost'
* @param params computed values to reduce overhead
* @param r Residual ( cost ) of term
*/
template<typename T>
inline void addCostResidual(
const double & weight,
const Eigen::Matrix<T, 2, 1> & pt,
const Eigen::Matrix<T, 2, 1> & pt_next,
const Eigen::Matrix<T, 2, 1> & pt_prev,
T & r ) const
{
if ( params_.cost_check_points.empty( ) ) {
Eigen::Matrix<T, 2, 1> interp_pos =
( pt - costmap_origin_.template cast<T>( ) ) / ( T )costmap_resolution_;
T value;
costmap_interpolator_->Evaluate( interp_pos[1] - ( T )0.5, interp_pos[0] - ( T )0.5, &value );
r += ( T )weight * value * value; // objective function value
} else {
Eigen::Matrix<T, 2, 1> dir = tangentDir(
pt_prev, pt, pt_next,
next_to_last_length_ratio_ < 0 );
dir.normalize( );
if ( ( ( pt_next - pt ).dot( dir ) < ( T )0 ) != reversing_ ) {
dir = -dir;
}
Eigen::Matrix<T, 3, 3> transform;
transform << dir[0], -dir[1], pt[0],
dir[1], dir[0], pt[1],
( T )0, ( T )0, ( T )1;
for ( size_t i = 0; i < params_.cost_check_points.size( ); i += 3 ) {
Eigen::Matrix<T, 3, 1> ccpt( ( T )params_.cost_check_points[i],
( T )params_.cost_check_points[i + 1], ( T )1 );
auto ccpt_world = ( transform * ccpt ).template block<2, 1>( 0, 0 );
Eigen::Matrix<T, 2,
1> interp_pos = ( ccpt_world - costmap_origin_.template cast<T>( ) ) /
( T )costmap_resolution_;
T value;
costmap_interpolator_->Evaluate( interp_pos[1] - ( T )0.5, interp_pos[0] - ( T )0.5, &value );
r += ( T )weight * ( T )params_.cost_check_points[i + 2] * value * value;
}
}
}
const Eigen::Vector2d original_pos_;
double next_to_last_length_ratio_;
bool reversing_;
SmootherParams params_;
double costmap_weight_;
Eigen::Vector2d costmap_origin_;
double costmap_resolution_;
std::shared_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>> costmap_interpolator_;
};
} // namespace nav2_constrained_smoother
#endif // NAV2_CONSTRAINED_SMOOTHER__SMOOTHER_COST_FUNCTION_HPP_
1 // Copyright ( c ) 2021 RoboTech Vision
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_CONSTRAINED_SMOOTHER__UTILS_HPP_
#define NAV2_CONSTRAINED_SMOOTHER__UTILS_HPP_
#include <limits>
#include "Eigen/Core"
#define EPSILON 0.0001
namespace nav2_constrained_smoother
{
/**
* @brief Center of an arc between three points
* @param pt_prev Starting point of the arc
* @param pt Mid point of the arc
* @param pt_next Last point of the arc
* @param is_cusp True if pt is a cusp point
* @result position of the center or Vector2( inf, inf ) for straight lines and 180 deg turns
*/
template<typename T>
35 inline Eigen::Matrix<T, 2, 1> arcCenter(
Eigen::Matrix<T, 2, 1> pt_prev,
Eigen::Matrix<T, 2, 1> pt,
Eigen::Matrix<T, 2, 1> pt_next,
bool is_cusp )
{
Eigen::Matrix<T, 2, 1> d1 = pt - pt_prev;
Eigen::Matrix<T, 2, 1> d2 = pt_next - pt;
if ( is_cusp ) {
d2 = -d2;
pt_next = pt + d2;
}
T det = d1[0] * d2[1] - d1[1] * d2[0];
if ( ceres::abs( det ) < ( T )1e-4 ) { // straight line
return Eigen::Matrix<T, 2, 1>(
( T )std::numeric_limits<double>::infinity( ), ( T )std::numeric_limits<double>::infinity( ) );
}
// circle center is at the intersection of mirror axes of the segments:
// http://paulbourke.net/geometry/circlesphere/
// line intersection:
// https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Intersection%20of%20two%20lines
Eigen::Matrix<T, 2, 1> mid1 = ( pt_prev + pt ) / ( T )2;
Eigen::Matrix<T, 2, 1> mid2 = ( pt + pt_next ) / ( T )2;
Eigen::Matrix<T, 2, 1> n1( -d1[1], d1[0] );
Eigen::Matrix<T, 2, 1> n2( -d2[1], d2[0] );
T det1 = ( mid1[0] + n1[0] ) * mid1[1] - ( mid1[1] + n1[1] ) * mid1[0];
T det2 = ( mid2[0] + n2[0] ) * mid2[1] - ( mid2[1] + n2[1] ) * mid2[0];
Eigen::Matrix<T, 2, 1> center( ( det1 * n2[0] - det2 * n1[0] ) / det,
( det1 * n2[1] - det2 * n1[1] ) / det );
return center;
}
/**
* @brief Direction of a line which contains pt and is tangential to arc
* between pt_prev, pt, pt_next
* @param pt_prev Starting point of the arc
* @param pt Mid point of the arc, lying on the tangential line
* @param pt_next Last point of the arc
* @param is_cusp True if pt is a cusp point
* @result Tangential line direction.
* Note: the sign of tangentDir is undefined here, should be assigned in post-process
* depending on movement direction. Also, for speed reasons, direction vector is not normalized.
*/
template<typename T>
82 inline Eigen::Matrix<T, 2, 1> tangentDir(
Eigen::Matrix<T, 2, 1> pt_prev,
Eigen::Matrix<T, 2, 1> pt,
Eigen::Matrix<T, 2, 1> pt_next,
bool is_cusp )
{
Eigen::Matrix<T, 2, 1> center = arcCenter( pt_prev, pt, pt_next, is_cusp );
if ( ceres::IsInfinite( center[0] ) ) { // straight line
Eigen::Matrix<T, 2, 1> d1 = pt - pt_prev;
Eigen::Matrix<T, 2, 1> d2 = pt_next - pt;
if ( is_cusp ) {
d2 = -d2;
pt_next = pt + d2;
}
Eigen::Matrix<T, 2, 1> result( pt_next[0] - pt_prev[0], pt_next[1] - pt_prev[1] );
if ( result[0] == 0.0 && result[1] == 0.0 ) { // a very rare edge situation
return Eigen::Matrix<T, 2, 1>( d1[1], -d1[0] );
}
return result;
}
// tangent is prependicular to ( pt - center )
// Note: not determining + or - direction here, this should be handled at the caller side
return Eigen::Matrix<T, 2, 1>( center[1] - pt[1], pt[0] - center[0] );
}
} // namespace nav2_constrained_smoother
#endif // NAV2_CONSTRAINED_SMOOTHER__UTILS_HPP_
1 // Copyright ( c ) 2021 RoboTech Vision
// Copyright ( c ) 2020 Shrijit Singh
// Copyright ( c ) 2020 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <algorithm>
#include <string>
#include <memory>
#include <utility>
#include <vector>
#include "nav2_constrained_smoother/constrained_smoother.hpp"
#include "nav2_core/exceptions.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
#include "pluginlib/class_loader.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "tf2/utils.h"
using nav2_util::declare_parameter_if_not_declared;
using nav2_util::geometry_utils::euclidean_distance;
using namespace nav2_costmap_2d; // NOLINT
namespace nav2_constrained_smoother
{
41 void ConstrainedSmoother::configure(
42 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
43 std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
44 std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub,
45 std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> )
{
auto node = parent.lock( );
if ( !node ) {
throw std::runtime_error( "Unable to lock node!" );
}
costmap_sub_ = costmap_sub;
tf_ = tf;
plugin_name_ = name;
logger_ = node->get_logger( );
smoother_ = std::make_unique<nav2_constrained_smoother::Smoother>( );
optimizer_params_.get( node.get( ), name );
smoother_params_.get( node.get( ), name );
smoother_->initialize( optimizer_params_ );
}
63 void ConstrainedSmoother::cleanup( )
{
RCLCPP_INFO(
logger_,
"Cleaning up smoother: %s of type"
" nav2_constrained_smoother::ConstrainedSmoother",
plugin_name_.c_str( ) );
}
72 void ConstrainedSmoother::activate( )
{
RCLCPP_INFO(
logger_,
"Activating smoother: %s of type "
"nav2_constrained_smoother::ConstrainedSmoother",
plugin_name_.c_str( ) );
}
81 void ConstrainedSmoother::deactivate( )
{
RCLCPP_INFO(
logger_,
"Deactivating smoother: %s of type "
"nav2_constrained_smoother::ConstrainedSmoother",
plugin_name_.c_str( ) );
}
90 bool ConstrainedSmoother::smooth( nav_msgs::msg::Path & path, const rclcpp::Duration & max_time )
{
if ( path.poses.size( ) < 2 ) {
return true;
}
// populate smoother input with ( x, y, forward/reverse dir )
std::vector<Eigen::Vector3d> path_world;
path_world.reserve( path.poses.size( ) );
// smoother keeps record of start/end orientations so that it
// can use them in the final path, preventing degradation of these ( often important ) values
Eigen::Vector2d start_dir;
Eigen::Vector2d end_dir;
for ( size_t i = 0; i < path.poses.size( ); i++ ) {
auto & pose = path.poses[i].pose;
double angle = tf2::getYaw( pose.orientation );
Eigen::Vector2d orientation( cos( angle ), sin( angle ) );
if ( i == path.poses.size( ) - 1 ) {
// Note: `reversing` indicates the direction of the segment after the point and
// there is no segment after the last point. Most probably the value is irrelevant, but
// copying it from the last but one point, just to make it defined...
path_world.emplace_back( pose.position.x, pose.position.y, path_world.back( )[2] );
end_dir = orientation;
} else {
auto & pos_next = path.poses[i + 1].pose.position;
Eigen::Vector2d mvmt( pos_next.x - pose.position.x, pos_next.y - pose.position.y );
// robot is considered reversing when angle between its orientation and movement direction
// is more than 90 degrees ( i.e. dot product is less than 0 )
bool reversing = smoother_params_.reversing_enabled && orientation.dot( mvmt ) < 0;
// we transform boolean value of "reversing" into sign of movement direction ( +1 or -1 )
// to simplify further computations
path_world.emplace_back( pose.position.x, pose.position.y, reversing ? -1 : 1 );
if ( i == 0 ) {
start_dir = orientation;
} else if ( i == 1 && !smoother_params_.keep_start_orientation ) {
// overwrite start forward/reverse when orientation was set to be ignored
// note: start_dir is overwritten inside Smoother::upsampleAndPopulate( ) method
path_world[0][2] = path_world.back( )[2];
}
}
}
smoother_params_.max_time = max_time.seconds( );
// Smooth plan
auto costmap = costmap_sub_->getCostmap( );
if ( !smoother_->smooth( path_world, start_dir, end_dir, costmap.get( ), smoother_params_ ) ) {
RCLCPP_WARN(
logger_,
"%s: failed to smooth plan, Ceres could not find a usable solution to optimize.",
plugin_name_.c_str( ) );
throw new nav2_core::PlannerException(
"Failed to smooth plan, Ceres could not find a usable solution." );
}
// populate final path
geometry_msgs::msg::PoseStamped pose;
pose.header = path.poses.front( ).header;
path.poses.clear( );
path.poses.reserve( path_world.size( ) );
for ( auto & pw : path_world ) {
pose.pose.position.x = pw[0];
pose.pose.position.y = pw[1];
pose.pose.orientation.z = sin( pw[2] / 2 );
pose.pose.orientation.w = cos( pw[2] / 2 );
path.poses.push_back( pose );
}
return true;
}
} // namespace nav2_constrained_smoother
// Register this smoother as a nav2_core plugin
165 PLUGINLIB_EXPORT_CLASS(
nav2_constrained_smoother::ConstrainedSmoother,
nav2_core::Smoother )
// Copyright ( c ) 2021 RoboTech Vision
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <string>
#include <memory>
#include <chrono>
#include <iostream>
#include <future>
#include <thread>
#include <algorithm>
#include <vector>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/create_timer_ros.h"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_costmap_2d/inflation_layer.hpp"
#include "nav2_costmap_2d/footprint_collision_checker.hpp"
#include "nav2_costmap_2d/costmap_2d_publisher.hpp"
#include "angles/angles.h"
#include "nav2_constrained_smoother/constrained_smoother.hpp"
#include "geometry_msgs/msg/pose_array.hpp"
37 class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber
{
public:
40 DummyCostmapSubscriber(
41 nav2_util::LifecycleNode::SharedPtr node,
42 const std::string & topic_name )
: CostmapSubscriber( node, topic_name )
{
auto costmap = std::make_shared<nav2_msgs::msg::Costmap>( );
costmap->metadata.size_x = 100;
costmap->metadata.size_y = 100;
costmap->metadata.resolution = 0.1;
costmap->metadata.origin.position.x = -5.0;
costmap->metadata.origin.position.y = -5.0;
costmap->data.resize( costmap->metadata.size_x * costmap->metadata.size_y, 0 );
// create an obstacle in rect ( 1.0, -1.0 ) -> ( 3.0, -2.0 )
// with inflation of radius 2.0
double cost_scaling_factor = 1.6;
double inscribed_radius = 0.3;
for ( int i = 10; i < 60; ++i ) {
for ( int j = 40; j < 100; ++j ) {
int dist_x = std::max( 0, std::max( 60 - j, j - 80 ) );
int dist_y = std::max( 0, std::max( 30 - i, i - 40 ) );
double dist = sqrt( dist_x * dist_x + dist_y * dist_y );
unsigned char cost;
if ( dist == 0 ) {
cost = nav2_costmap_2d::LETHAL_OBSTACLE;
} else if ( dist < inscribed_radius ) {
cost = nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
} else {
double factor =
exp(
-1.0 * cost_scaling_factor * ( dist * costmap->metadata.resolution - inscribed_radius ) );
cost =
static_cast<unsigned char>( ( nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1 ) * factor );
}
costmap->data[i * costmap->metadata.size_x + j] = cost;
}
}
setCostmap( costmap );
}
82 void setCostmap( nav2_msgs::msg::Costmap::SharedPtr msg )
{
costmap_msg_ = msg;
costmap_received_ = true;
}
};
89 geometry_msgs::msg::Point pointMsg( double x, double y )
{
geometry_msgs::msg::Point point;
point.x = x;
point.y = y;
return point;
}
97 class SmootherTest : public ::testing::Test
{
protected:
100 SmootherTest( ) {SetUp( );}
101 ~SmootherTest( ) {}
103 void SetUp( )
{
node_lifecycle_ =
std::make_shared<rclcpp_lifecycle::LifecycleNode>(
"ConstrainedSmootherTestNode", rclcpp::NodeOptions( ) );
tf_buffer_ = std::make_shared<tf2_ros::Buffer>( node_lifecycle_->get_clock( ) );
auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
node_lifecycle_->get_node_base_interface( ),
node_lifecycle_->get_node_timers_interface( ) );
tf_buffer_->setCreateTimerInterface( timer_interface );
costmap_sub_ =
std::make_shared<DummyCostmapSubscriber>(
node_lifecycle_, "costmap_topic" );
path_poses_pub_ = node_lifecycle_->create_publisher<geometry_msgs::msg::PoseArray>(
"/plan_poses_optimized", 100 );
path_poses_pub_cmp_ = node_lifecycle_->create_publisher<geometry_msgs::msg::PoseArray>(
"/plan_poses_optimized_cmp", 100 );
path_poses_pub_orig_ = node_lifecycle_->create_publisher<geometry_msgs::msg::PoseArray>(
"/plan_poses_original", 100 );
costmap_pub_ = std::make_shared<nav2_costmap_2d::Costmap2DPublisher>(
node_lifecycle_,
costmap_sub_->getCostmap( ).get( ), "map", "/costmap", true );
node_lifecycle_->configure( );
node_lifecycle_->activate( );
path_poses_pub_->on_activate( );
path_poses_pub_cmp_->on_activate( );
path_poses_pub_orig_->on_activate( );
costmap_pub_->on_activate( );
smoother_ = std::make_shared<nav2_constrained_smoother::ConstrainedSmoother>( );
smoother_->configure(
node_lifecycle_, "SmoothPath", tf_buffer_, costmap_sub_,
std::shared_ptr<nav2_costmap_2d::FootprintSubscriber>( ) );
smoother_->activate( );
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.w_smooth", 15000.0 ) );
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.minimum_turning_radius", 0.4 ) );
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.w_curve", 30.0 ) );
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.w_dist", 0.0 ) );
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.w_cost", 0.0 ) );
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.cusp_zone_length", -1.0 ) );
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.w_cost_cusp_multiplier", 1.0 ) );
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.path_downsampling_factor", 1 ) );
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.path_upsampling_factor", 1 ) );
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.reversing_enabled", true ) );
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.keep_start_orientation", true ) );
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.keep_goal_orientation", true ) );
node_lifecycle_->set_parameter(
rclcpp::Parameter( "SmoothPath.optimizer.linear_solver_type", "SPARSE_NORMAL_CHOLESKY" ) );
node_lifecycle_->set_parameter(
rclcpp::Parameter(
"SmoothPath.cost_check_points",
std::vector<double>( ) ) );
reloadParams( );
}
void TearDown( ) override
{
smoother_->deactivate( );
smoother_->cleanup( );
path_poses_pub_->on_deactivate( );
path_poses_pub_cmp_->on_deactivate( );
path_poses_pub_orig_->on_deactivate( );
costmap_pub_->on_deactivate( );
node_lifecycle_->deactivate( );
}
void reloadParams( )
{
smoother_->deactivate( );
179 smoother_->cleanup( );
180 smoother_->configure(
node_lifecycle_, "SmoothPath", tf_buffer_, costmap_sub_,
std::shared_ptr<nav2_costmap_2d::FootprintSubscriber>( ) );
smoother_->activate( );
}
bool smoothPath(
const std::vector<Eigen::Vector3d> & input, std::vector<Eigen::Vector3d> & output,
bool publish = false, bool cmp = false )
{
nav_msgs::msg::Path path;
path.poses.reserve( input.size( ) );
for ( auto & xya : input ) {
geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x = xya.x( );
pose.pose.position.y = xya.y( );
pose.pose.position.z = 0;
pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( xya.z( ) );
path.poses.push_back( pose );
}
if ( publish && !path.poses.empty( ) ) {
geometry_msgs::msg::PoseArray poses;
poses.header.frame_id = "map";
poses.header.stamp = node_lifecycle_->get_clock( )->now( );
for ( auto & p : path.poses ) {
poses.poses.push_back( p.pose );
}
path_poses_pub_orig_->publish( poses );
costmap_pub_->publishCostmap( );
}
bool result = smoother_->smooth( path, rclcpp::Duration::from_seconds( 10.0 ) );
if ( publish && !path.poses.empty( ) ) {
geometry_msgs::msg::PoseArray poses;
poses.header.frame_id = "map";
poses.header.stamp = node_lifecycle_->get_clock( )->now( );
for ( auto & p : path.poses ) {
poses.poses.push_back( p.pose );
}
auto & pub = cmp ? path_poses_pub_cmp_ : path_poses_pub_;
pub->publish( poses );
}
output.clear( );
output.reserve( path.poses.size( ) );
for ( auto & pose : path.poses ) {
Eigen::Vector3d xya;
xya.x( ) = pose.pose.position.x;
xya.y( ) = pose.pose.position.y;
tf2::Quaternion q;
tf2::fromMsg( pose.pose.orientation, q );
xya.z( ) = q.getAngle( );
output.push_back( xya );
}
return result;
}
typedef std::function<double ( int i, const Eigen::Vector3d & prev_p, const Eigen::Vector3d & p,
const Eigen::Vector3d & next_p )> QualityCriterion3;
typedef std::function<double ( int i, const Eigen::Vector3d & prev_p,
const Eigen::Vector3d & p )> QualityCriterion2;
typedef std::function<double ( int i, const Eigen::Vector3d & p )> QualityCriterion1;
/**
* @brief Path improvement assessment method
* @param input Smoother input path
* @param output Smoother output path
* @param criterion Criterion of path quality. Total path quality = sqrt( sum( criterion( data[i] )^2 )/count( data ) )
* @return Percentage of improvement ( relative to input path quality )
*/
double assessPathImprovement(
const std::vector<Eigen::Vector3d> & input,
const std::vector<Eigen::Vector3d> & output,
const QualityCriterion3 & criterion,
const QualityCriterion3 * criterion_out = nullptr )
{
if ( !criterion_out ) {
criterion_out = &criterion;
}
double total_input_crit = 0.0;
for ( size_t i = 1; i < input.size( ) - 1; i++ ) {
double input_crit = criterion( i, input[i - 1], input[i], input[i + 1] );
total_input_crit += input_crit * input_crit;
}
total_input_crit = sqrt( total_input_crit / ( input.size( ) - 2 ) );
double total_output_crit = 0.0;
for ( size_t i = 1; i < output.size( ) - 1; i++ ) {
double output_crit = ( *criterion_out )( i, output[i - 1], output[i], output[i + 1] );
total_output_crit += output_crit * output_crit;
}
total_output_crit = sqrt( total_output_crit / ( input.size( ) - 2 ) );
return ( 1.0 - total_output_crit / total_input_crit ) * 100;
}
/**
* @brief Path improvement assessment method
* @param input Smoother input path
* @param output Smoother output path
* @param criterion Criterion of path quality. Total path quality = sqrt( sum( criterion( data[i] )^2 )/count( data ) )
* @return Percentage of improvement ( relative to input path quality )
*/
double assessPathImprovement(
const std::vector<Eigen::Vector3d> & input,
const std::vector<Eigen::Vector3d> & output,
const QualityCriterion2 & criterion,
const QualityCriterion2 * criterion_out = nullptr )
{
if ( !criterion_out ) {
criterion_out = &criterion;
}
double total_input_crit = 0.0;
for ( size_t i = 1; i < input.size( ); i++ ) {
double input_crit = criterion( i, input[i - 1], input[i] );
total_input_crit += input_crit * input_crit;
}
total_input_crit = sqrt( total_input_crit / ( input.size( ) - 1 ) );
double total_output_crit = 0.0;
for ( size_t i = 1; i < output.size( ); i++ ) {
double output_crit = ( *criterion_out )( i, output[i - 1], output[i] );
total_output_crit += output_crit * output_crit;
}
total_output_crit = sqrt( total_output_crit / ( output.size( ) - 1 ) );
return ( 1.0 - total_output_crit / total_input_crit ) * 100;
}
/**
* @brief Path improvement assessment method
* @param input Smoother input path
* @param output Smoother output path
* @param criterion Criterion of path quality. Total path quality = sqrt( sum( criterion( data[i] )^2 )/count( data ) )
* @return Percentage of improvement ( relative to input path quality )
*/
double assessPathImprovement(
const std::vector<Eigen::Vector3d> & input,
const std::vector<Eigen::Vector3d> & output,
const QualityCriterion1 & criterion,
const QualityCriterion1 * criterion_out = nullptr )
{
if ( !criterion_out ) {
criterion_out = &criterion;
}
double total_input_crit = 0.0;
for ( size_t i = 0; i < input.size( ); i++ ) {
double input_crit = criterion( i, input[i] );
total_input_crit += input_crit * input_crit;
}
total_input_crit = sqrt( total_input_crit / input.size( ) );
double total_output_crit = 0.0;
for ( size_t i = 0; i < output.size( ); i++ ) {
double output_crit = ( *criterion_out )( i, output[i] );
total_output_crit += output_crit * output_crit;
}
total_output_crit = sqrt( total_output_crit / output.size( ) );
return ( 1.0 - total_output_crit / total_input_crit ) * 100;
}
/**
* @brief Worst pose improvement assessment method
* @param input Smoother input path
* @param output Smoother output path
* @param criterion Criterion of path quality. Total path quality = max( criterion( data[i] ) )
* @return Percentage of improvement ( relative to input path quality )
*/
double assessWorstPoseImprovement(
const std::vector<Eigen::Vector3d> & input,
const std::vector<Eigen::Vector3d> & output,
const QualityCriterion1 & criterion )
{
double max_input_crit = 0.0;
for ( size_t i = 0; i < input.size( ); i++ ) {
double input_crit = criterion( i, input[i] );
max_input_crit = std::max( max_input_crit, input_crit );
}
double max_output_crit = 0.0;
for ( size_t i = 0; i < output.size( ); i++ ) {
double output_crit = criterion( i, output[i] );
max_output_crit = std::max( max_output_crit, output_crit );
}
return ( 1.0 - max_output_crit / max_input_crit ) * 100;
}
std::vector<Eigen::Vector3d> zigZaggedPath(
const std::vector<Eigen::Vector3d> & input,
double offset )
{
auto output = input;
for ( size_t i = 1; i < input.size( ) - 1; i++ ) {
// add offset prependicular to path
Eigen::Vector2d direction =
( input[i + 1].block<2, 1>( 0, 0 ) - input[i - 1].block<2, 1>( 0, 0 ) ).normalized( );
output[i].block<2, 1>(
0,
0 ) += Eigen::Vector2d( direction[1], -direction[0] ) * offset * ( i % 2 == 0 ? 1.0 : -1.0 );
}
return output;
}
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_lifecycle_;
std::shared_ptr<nav2_constrained_smoother::ConstrainedSmoother> smoother_;
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<DummyCostmapSubscriber> costmap_sub_;
std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr
path_poses_pub_orig_;
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr path_poses_pub_;
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr
path_poses_pub_cmp_;
std::shared_ptr<nav2_costmap_2d::Costmap2DPublisher> costmap_pub_;
int cusp_i_ = -1;
QualityCriterion3 mvmt_smoothness_criterion_ =
[this]( int i, const Eigen::Vector3d & prev_p, const Eigen::Vector3d & p,
const Eigen::Vector3d & next_p ) {
Eigen::Vector2d prev_mvmt = p.block<2, 1>( 0, 0 ) - prev_p.block<2, 1>( 0, 0 );
Eigen::Vector2d next_mvmt = next_p.block<2, 1>( 0, 0 ) - p.block<2, 1>( 0, 0 );
if ( i == cusp_i_ ) {
next_mvmt = -next_mvmt;
}
return ( next_mvmt - prev_mvmt ).norm( );
};
};
TEST_F( SmootherTest, testingSmoothness )
{
// set w_curve to 0.0 so that the whole job is upon w_smooth
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.w_curve", 0.0 ) );
reloadParams( );
std::vector<Eigen::Vector3d> sharp_turn_90 =
{{0, 0, 0},
{0.1, 0, 0},
{0.2, 0, 0},
{0.3, 0, M_PI / 4},
{0.3, 0.1, M_PI / 2},
{0.3, 0.2, M_PI / 2},
{0.3, 0.3, M_PI / 2}
};
std::vector<Eigen::Vector3d> smoothed_path;
EXPECT_TRUE( smoothPath( sharp_turn_90, smoothed_path ) );
double mvmt_smoothness_improvement =
assessPathImprovement( sharp_turn_90, smoothed_path, mvmt_smoothness_criterion_ );
EXPECT_GT( mvmt_smoothness_improvement, 0.0 );
EXPECT_NEAR( mvmt_smoothness_improvement, 55.3, 1.0 );
auto orientation_smoothness_criterion =
[]( int, const Eigen::Vector3d & prev_p, const Eigen::Vector3d & p ) {
return angles::normalize_angle( p.z( ) - prev_p.z( ) );
};
double orientation_smoothness_improvement =
assessPathImprovement( sharp_turn_90, smoothed_path, orientation_smoothness_criterion );
EXPECT_GT( orientation_smoothness_improvement, 0.0 );
EXPECT_NEAR( orientation_smoothness_improvement, 38.7, 1.0 );
// path with a cusp
std::vector<Eigen::Vector3d> sharp_turn_90_then_reverse =
{{0, 0, 0},
{0.1, 0, 0},
{0.2, 0, 0},
{0.3, 0, 0},
{0.4, 0, 0},
{0.5, 0, 0},
{0.6, 0, M_PI / 4},
{0.6, -0.1, M_PI / 2},
{0.6, -0.2, M_PI / 2},
{0.6, -0.3, M_PI / 2},
{0.6, -0.4, M_PI / 2},
{0.6, -0.5, M_PI / 2},
{0.6, -0.6, M_PI / 2}
};
cusp_i_ = 6;
EXPECT_TRUE( smoothPath( sharp_turn_90_then_reverse, smoothed_path ) );
mvmt_smoothness_improvement =
assessPathImprovement( sharp_turn_90_then_reverse, smoothed_path, mvmt_smoothness_criterion_ );
EXPECT_GT( mvmt_smoothness_improvement, 0.0 );
EXPECT_NEAR( mvmt_smoothness_improvement, 37.2, 1.0 );
orientation_smoothness_improvement =
assessPathImprovement(
sharp_turn_90_then_reverse, smoothed_path,
orientation_smoothness_criterion );
EXPECT_GT( orientation_smoothness_improvement, 0.0 );
EXPECT_NEAR( orientation_smoothness_improvement, 28.5, 1.0 );
SUCCEED( );
}
TEST_F( SmootherTest, testingAnchoringToOriginalPath )
{
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.w_smooth", 30.0 ) );
// set w_curve to 0.0, we don't care about turning radius in this test...
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.w_curve", 0.0 ) );
// first keep w_dist at 0.0 to generate an unanchored smoothed path
reloadParams( );
std::vector<Eigen::Vector3d> sharp_turn_90 =
{{0, 0, 0},
{0.1, 0, 0},
{0.2, 0, 0},
{0.3, 0, M_PI / 4},
{0.3, 0.1, M_PI / 2},
{0.3, 0.2, M_PI / 2},
{0.3, 0.3, M_PI / 2}
};
std::vector<Eigen::Vector3d> smoothed_path;
EXPECT_TRUE( smoothPath( sharp_turn_90, smoothed_path ) );
// then update w_dist and compare the results
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.w_dist", 30.0 ) );
reloadParams( );
std::vector<Eigen::Vector3d> smoothed_path_anchored;
EXPECT_TRUE( smoothPath( sharp_turn_90, smoothed_path_anchored ) );
auto origin_similarity_criterion =
[&sharp_turn_90]( int i, const Eigen::Vector3d & p ) {
return ( p.block<2, 1>( 0, 0 ) - sharp_turn_90[i].block<2, 1>( 0, 0 ) ).norm( );
};
double origin_similarity_improvement =
assessPathImprovement( smoothed_path, smoothed_path_anchored, origin_similarity_criterion );
EXPECT_GT( origin_similarity_improvement, 0.0 );
EXPECT_NEAR( origin_similarity_improvement, 45.5, 1.0 );
SUCCEED( );
}
TEST_F( SmootherTest, testingMaxCurvature )
{
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.w_curve", 30.0 ) );
// set w_smooth to a small value so that the whole job is upon w_curve
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.w_smooth", 0.3 ) );
// let's give the smoother more time since w_smooth is so small
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.optimizer.max_iterations", 500 ) );
reloadParams( );
// smoother should increase radius from infeasible 0.3 to feasible 0.4
std::vector<Eigen::Vector3d> radius_0_3_turn_90 =
{{0, 0, 0},
{0.1, 0, 0},
{0.2, 0, 0},
{0.2 + 0.3 * sin( M_PI / 12 ), 0.3 * ( 1 - cos( M_PI / 12 ) ), 0},
{0.2 + 0.3 * sin( M_PI * 2 / 12 ), 0.3 * ( 1 - cos( M_PI * 2 / 12 ) ), 0},
{0.2 + 0.3 * sin( M_PI * 3 / 12 ), 0.3 * ( 1 - cos( M_PI * 3 / 12 ) ), 0},
{0.2 + 0.3 * sin( M_PI * 4 / 12 ), 0.3 * ( 1 - cos( M_PI * 4 / 12 ) ), 0},
{0.2 + 0.3 * sin( M_PI * 5 / 12 ), 0.3 * ( 1 - cos( M_PI * 5 / 12 ) ), 0},
{0.5, 0.3, M_PI / 2},
{0.5, 0.4, M_PI / 2},
{0.5, 0.5, M_PI / 2}
};
std::vector<Eigen::Vector3d> smoothed_path;
EXPECT_TRUE( smoothPath( radius_0_3_turn_90, smoothed_path ) );
// we don't expect result to be smoother than original as w_smooth is too low
// but let's check for large discontinuities using a well chosen upper bound
auto upper_bound = zigZaggedPath( radius_0_3_turn_90, 0.01 );
EXPECT_GT( assessPathImprovement( upper_bound, smoothed_path, mvmt_smoothness_criterion_ ), 0.0 );
// smoothed path points should form a circle with radius 0.4
for ( size_t i = 1; i < smoothed_path.size( ) - 1; i++ ) {
auto & p = smoothed_path[i];
double r = ( p.block<2, 1>( 0, 0 ) - Eigen::Vector2d( 0.1, 0.4 ) ).norm( );
EXPECT_NEAR( r, 0.4, 0.01 );
}
// path with a cusp
// smoother should increase radius from infeasible 0.3 to feasible 0.4
std::vector<Eigen::Vector3d> radius_0_3_turn_90_then_reverse_turn_90 =
{{0, 0, 0},
{0.1, 0, 0},
{0.2, 0, 0},
{0.2 + 0.3 * sin( M_PI / 12 ), 0.3 * ( 1 - cos( M_PI / 12 ) ), M_PI / 12},
{0.2 + 0.3 * sin( M_PI * 2 / 12 ), 0.3 * ( 1 - cos( M_PI * 2 / 12 ) ), M_PI *2 / 12},
{0.2 + 0.3 * sin( M_PI * 3 / 12 ), 0.3 * ( 1 - cos( M_PI * 3 / 12 ) ), M_PI *3 / 12},
{0.2 + 0.3 * sin( M_PI * 4 / 12 ), 0.3 * ( 1 - cos( M_PI * 4 / 12 ) ), M_PI *4 / 12},
{0.2 + 0.3 * sin( M_PI * 5 / 12 ), 0.3 * ( 1 - cos( M_PI * 5 / 12 ) ), M_PI *5 / 12},
{0.5, 0.3, M_PI / 2},
{0.8 - 0.3 * sin( M_PI * 5 / 12 ), 0.3 * ( 1 - cos( M_PI * 5 / 12 ) ), M_PI *7 / 12},
{0.8 - 0.3 * sin( M_PI * 4 / 12 ), 0.3 * ( 1 - cos( M_PI * 4 / 12 ) ), M_PI *8 / 12},
{0.8 - 0.3 * sin( M_PI * 3 / 12 ), 0.3 * ( 1 - cos( M_PI * 3 / 12 ) ), M_PI *9 / 12},
{0.8 - 0.3 * sin( M_PI * 2 / 12 ), 0.3 * ( 1 - cos( M_PI * 2 / 12 ) ), M_PI *10 / 12},
{0.8 - 0.3 * sin( M_PI / 12 ), 0.3 * ( 1 - cos( M_PI / 12 ) ), M_PI *11 / 12},
{0.8, 0, M_PI},
{0.9, 0, M_PI},
{1.0, 0, M_PI}
};
EXPECT_TRUE( smoothPath( radius_0_3_turn_90_then_reverse_turn_90, smoothed_path ) );
// we don't expect result to be smoother than original as w_smooth is too low
// but let's check for large discontinuities using a well chosen upper bound
cusp_i_ = 8;
upper_bound = zigZaggedPath( radius_0_3_turn_90_then_reverse_turn_90, 0.01 );
EXPECT_GT( assessPathImprovement( upper_bound, smoothed_path, mvmt_smoothness_criterion_ ), 0.0 );
// smoothed path points should form a circle with radius 0.4
// for both forward and reverse movements
for ( size_t i = 1; i < smoothed_path.size( ) - 1; i++ ) {
auto & p = smoothed_path[i];
double r = ( p.block<2, 1>( 0, 0 ) - Eigen::Vector2d( i <= 8 ? 0.1 : 0.9, 0.4 ) ).norm( );
EXPECT_NEAR( r, 0.4, 0.01 );
}
SUCCEED( );
}
TEST_F( SmootherTest, testingObstacleAvoidance )
{
auto costmap = costmap_sub_->getCostmap( );
nav2_costmap_2d::FootprintCollisionChecker collision_checker( costmap );
nav2_costmap_2d::Footprint footprint;
auto cost_avoidance_criterion =
[&collision_checker, &footprint]( int, const Eigen::Vector3d & p ) {
return collision_checker.footprintCostAtPose( p[0], p[1], p[2], footprint );
};
// a symmetric footprint ( diff-drive with 4 actuated wheels )
footprint.push_back( pointMsg( 0.4, 0.25 ) );
footprint.push_back( pointMsg( -0.4, 0.25 ) );
footprint.push_back( pointMsg( -0.4, -0.25 ) );
footprint.push_back( pointMsg( 0.4, -0.25 ) );
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.w_smooth", 15000.0 ) );
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.w_cost", 0.015 ) );
reloadParams( );
std::vector<Eigen::Vector3d> straight_near_obstacle =
{{0.05, 0.05, 0},
{0.45, 0.05, 0},
{0.85, 0.05, 0},
{1.25, 0.05, 0},
{1.65, 0.05, 0},
{2.05, 0.05, 0},
{2.45, 0.05, 0},
{2.85, 0.05, 0},
{3.25, 0.05, 0},
{3.65, 0.05, 0},
{4.05, 0.05, 0}
};
std::vector<Eigen::Vector3d> smoothed_path;
EXPECT_TRUE( smoothPath( straight_near_obstacle, smoothed_path ) );
// we don't expect result to be smoother than original as original straight line was 100% smooth
// but let's check for large discontinuities using a well chosen upper bound
auto upper_bound = zigZaggedPath( straight_near_obstacle, 0.01 );
EXPECT_GT( assessPathImprovement( upper_bound, smoothed_path, mvmt_smoothness_criterion_ ), 0.0 );
double cost_avoidance_improvement = assessPathImprovement(
straight_near_obstacle, smoothed_path,
cost_avoidance_criterion );
EXPECT_GT( cost_avoidance_improvement, 0.0 );
EXPECT_NEAR( cost_avoidance_improvement, 12.9, 1.0 );
}
TEST_F( SmootherTest, testingObstacleAvoidanceNearCusps )
{
auto costmap = costmap_sub_->getCostmap( );
nav2_costmap_2d::FootprintCollisionChecker collision_checker( costmap );
nav2_costmap_2d::Footprint footprint;
auto cost_avoidance_criterion =
[&collision_checker, &footprint]( int, const Eigen::Vector3d & p ) {
return collision_checker.footprintCostAtPose( p[0], p[1], p[2], footprint );
};
// path with a cusp
std::vector<Eigen::Vector3d> cusp_near_obstacle =
{{0.05, 0.05, 0},
{0.15, 0.05, 0},
{0.25, 0.05, 0},
{0.35, 0.05, 0},
{0.45, 0.05, 0},
{0.55, 0.05, 0},
{0.65, 0.05, 0},
{0.75, 0.05, 0},
{0.85, 0.05, 0},
{0.95, 0.05, 0},
{1.05, 0.05, 0},
{1.15, 0.05, 0},
{1.25, 0.05, 0},
{1.25 + 0.4 * sin( M_PI / 12 ), 0.4 * ( 1 - cos( M_PI / 12 ) ) + 0.05, M_PI / 12},
{1.25 + 0.4 * sin( M_PI * 2 / 12 ), 0.4 * ( 1 - cos( M_PI * 2 / 12 ) ) + 0.05, M_PI *2 / 12},
{1.25 + 0.4 * sin( M_PI * 3 / 12 ), 0.4 * ( 1 - cos( M_PI * 3 / 12 ) ) + 0.05, M_PI *3 / 12},
{1.25 + 0.4 * sin( M_PI * 4 / 12 ), 0.4 * ( 1 - cos( M_PI * 4 / 12 ) ) + 0.05, M_PI *4 / 12},
{1.25 + 0.4 * sin( M_PI * 5 / 12 ), 0.4 * ( 1 - cos( M_PI * 5 / 12 ) ) + 0.05, M_PI *5 / 12},
{1.65, 0.45, M_PI / 2},
{2.05 - 0.4 * sin( M_PI * 5 / 12 ), 0.4 * ( 1 - cos( M_PI * 5 / 12 ) ) + 0.05, M_PI *7 / 12},
{2.05 - 0.4 * sin( M_PI * 4 / 12 ), 0.4 * ( 1 - cos( M_PI * 4 / 12 ) ) + 0.05, M_PI *8 / 12},
{2.05 - 0.4 * sin( M_PI * 3 / 12 ), 0.4 * ( 1 - cos( M_PI * 3 / 12 ) ) + 0.05, M_PI *9 / 12},
{2.05 - 0.4 * sin( M_PI * 2 / 12 ), 0.4 * ( 1 - cos( M_PI * 2 / 12 ) ) + 0.05, M_PI *10 / 12},
{2.05 - 0.4 * sin( M_PI / 12 ), 0.4 * ( 1 - cos( M_PI / 12 ) ) + 0.05, M_PI *11 / 12},
{2.05, 0.05, M_PI},
{2.15, 0.05, M_PI},
{2.25, 0.05, M_PI},
{2.35, 0.05, M_PI},
{2.45, 0.05, M_PI},
{2.55, 0.05, M_PI},
{2.65, 0.05, M_PI},
{2.75, 0.05, M_PI},
{2.85, 0.05, M_PI},
{2.95, 0.05, M_PI},
{3.05, 0.05, M_PI},
{3.15, 0.05, M_PI},
{3.25, 0.05, M_PI},
{3.35, 0.05, M_PI},
{3.45, 0.05, M_PI},
{3.55, 0.05, M_PI},
{3.65, 0.05, M_PI},
{3.75, 0.05, M_PI},
{3.85, 0.05, M_PI},
{3.95, 0.05, M_PI},
{4.05, 0.05, M_PI}
};
cusp_i_ = 18;
// we don't expect result to be smoother than original
// but let's check for large discontinuities using a well chosen upper bound
auto upper_bound = zigZaggedPath( cusp_near_obstacle, 0.01 );
/////////////////////////////////////////////////////
// testing option to pay extra attention near cusps
// extra attention near cusps option is more significant with a long footprint
footprint.clear( );
footprint.push_back( pointMsg( 0.4, 0.2 ) );
footprint.push_back( pointMsg( -0.4, 0.2 ) );
footprint.push_back( pointMsg( -0.4, -0.2 ) );
footprint.push_back( pointMsg( 0.4, -0.2 ) );
// first smooth with homogeneous w_cost to compare
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.w_cost", 0.015 ) );
// higher w_curve significantly decreases convergence speed here
// path feasibility can be restored by subsequent resmoothing with higher w_curve
// TODO( afrixs ): tune ceres optimizer to "converge" faster,
// see http://ceres-solver.org/nnls_solving.html
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.w_curve", 1.0 ) );
// let's have more iterations so that the improvement is more significant
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.optimizer.max_iterations", 500 ) );
reloadParams( );
std::vector<Eigen::Vector3d> smoothed_path;
EXPECT_TRUE( smoothPath( cusp_near_obstacle, smoothed_path, true, true ) );
EXPECT_GT( assessPathImprovement( upper_bound, smoothed_path, mvmt_smoothness_criterion_ ), 0.0 );
double cost_avoidance_improvement_simple = assessPathImprovement(
cusp_near_obstacle,
smoothed_path,
cost_avoidance_criterion );
EXPECT_GT( cost_avoidance_improvement_simple, 0.0 );
EXPECT_NEAR( cost_avoidance_improvement_simple, 42.6, 1.0 );
double worst_cost_improvement_simple = assessWorstPoseImprovement(
cusp_near_obstacle,
smoothed_path,
cost_avoidance_criterion );
RCLCPP_INFO(
rclcpp::get_logger( "ceres_smoother" ), "Cost avoidance improvement ( cusp, simple ): %lf, %lf",
cost_avoidance_improvement_simple, worst_cost_improvement_simple );
EXPECT_GE( worst_cost_improvement_simple, 0.0 );
// then update parameters so that robot is not so afraid of obstacles
// during simple movement but pays extra attention during rotations near cusps
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.w_cost", 0.0052 ) );
node_lifecycle_->set_parameter(
rclcpp::Parameter( "SmoothPath.w_cost_cusp_multiplier", 0.027 / 0.0052 ) );
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.cusp_zone_length", 2.5 ) );
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.optimizer.fn_tol", 1e-15 ) );
reloadParams( );
std::vector<Eigen::Vector3d> smoothed_path_ecc;
EXPECT_TRUE( smoothPath( cusp_near_obstacle, smoothed_path_ecc, true, false ) );
EXPECT_GT( assessPathImprovement( upper_bound, smoothed_path_ecc, mvmt_smoothness_criterion_ ), 0.0 );
double cost_avoidance_improvement_extra_careful_cusp = assessPathImprovement(
cusp_near_obstacle,
smoothed_path_ecc,
cost_avoidance_criterion );
EXPECT_GT( cost_avoidance_improvement_extra_careful_cusp, 0.0 );
EXPECT_NEAR( cost_avoidance_improvement_extra_careful_cusp, 44.2, 1.0 );
double worst_cost_improvement_extra_careful_cusp = assessWorstPoseImprovement(
cusp_near_obstacle,
smoothed_path_ecc,
cost_avoidance_criterion );
RCLCPP_INFO(
rclcpp::get_logger( "ceres_smoother" ), "Cost avoidance improvement ( cusp, ecc ): %lf, %lf",
cost_avoidance_improvement_extra_careful_cusp, worst_cost_improvement_extra_careful_cusp );
EXPECT_GE( worst_cost_improvement_extra_careful_cusp, 0.0 );
EXPECT_GE( worst_cost_improvement_extra_careful_cusp, worst_cost_improvement_simple );
EXPECT_GT( cost_avoidance_improvement_extra_careful_cusp, cost_avoidance_improvement_simple );
// although extra careful cusp optimization avoids cost better than simple one,
// overall the path doesn't need to deflect so much from original, since w_cost is smaller
// and thus the obstacles are avoided mostly in dangerous zones around cusps
auto origin_similarity_criterion =
[&cusp_near_obstacle]( int i, const Eigen::Vector3d & p ) {
return ( p.block<2, 1>( 0, 0 ) - cusp_near_obstacle[i].block<2, 1>( 0, 0 ) ).norm( );
};
double origin_similarity_improvement =
assessPathImprovement( smoothed_path, smoothed_path_ecc, origin_similarity_criterion );
RCLCPP_INFO(
rclcpp::get_logger(
"ceres_smoother" ), "Original similarity improvement ( cusp, ecc vs. simple ): %lf",
origin_similarity_improvement );
EXPECT_GT( origin_similarity_improvement, 0.0 );
EXPECT_NEAR( origin_similarity_improvement, 0.43, 0.02 );
/////////////////////////////////////////////////////
// testing asymmetric footprint options
// ( diff-drive with 2 actuated wheels and 2 caster wheels )
footprint.clear( );
footprint.push_back( pointMsg( 0.15, 0.2 ) );
footprint.push_back( pointMsg( -0.65, 0.2 ) );
footprint.push_back( pointMsg( -0.65, -0.2 ) );
footprint.push_back( pointMsg( 0.15, -0.2 ) );
// reset parameters back to homogeneous and shift cost check point to the center of the footprint
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.w_smooth", 15000 ) );
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.w_curve", 1.0 ) );
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.w_cost", 0.015 ) );
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.cusp_zone_length", -1.0 ) );
node_lifecycle_->set_parameter(
rclcpp::Parameter(
"SmoothPath.cost_check_points",
std::vector<double>( {-0.05, 0.0, 0.5, -0.45, 0.0, 0.5} ) // x1, y1, weight1, x2, y2, weight2
) );
reloadParams( );
// cost improvement is different for path smoothed by original optimizer
// since the footprint has changed
cost_avoidance_improvement_simple = assessPathImprovement(
cusp_near_obstacle, smoothed_path,
cost_avoidance_criterion );
worst_cost_improvement_simple = assessWorstPoseImprovement(
cusp_near_obstacle, smoothed_path,
cost_avoidance_criterion );
EXPECT_GT( cost_avoidance_improvement_simple, 0.0 );
RCLCPP_INFO(
rclcpp::get_logger(
"ceres_smoother" ), "Cost avoidance improvement ( cusp_shifted, simple ): %lf, %lf",
cost_avoidance_improvement_simple, worst_cost_improvement_simple );
EXPECT_NEAR( cost_avoidance_improvement_simple, 40.2, 1.0 );
// now smooth using the new optimizer with cost check point shifted
std::vector<Eigen::Vector3d> smoothed_path_scc;
EXPECT_TRUE( smoothPath( cusp_near_obstacle, smoothed_path_scc ) );
EXPECT_GT( assessPathImprovement( upper_bound, smoothed_path_scc, mvmt_smoothness_criterion_ ), 0.0 );
double cost_avoidance_improvement_shifted_cost_check = assessPathImprovement(
cusp_near_obstacle,
smoothed_path_scc,
cost_avoidance_criterion );
EXPECT_GT( cost_avoidance_improvement_shifted_cost_check, 0.0 );
EXPECT_NEAR( cost_avoidance_improvement_shifted_cost_check, 42.0, 1.0 );
double worst_cost_improvement_shifted_cost_check = assessWorstPoseImprovement(
cusp_near_obstacle,
smoothed_path_scc,
cost_avoidance_criterion );
RCLCPP_INFO(
rclcpp::get_logger(
"ceres_smoother" ), "Cost avoidance improvement ( cusp_shifted, scc ): %lf, %lf",
cost_avoidance_improvement_shifted_cost_check, worst_cost_improvement_shifted_cost_check );
EXPECT_GE( worst_cost_improvement_shifted_cost_check, 0.0 );
EXPECT_GE( worst_cost_improvement_shifted_cost_check, worst_cost_improvement_simple );
EXPECT_GT( cost_avoidance_improvement_shifted_cost_check, cost_avoidance_improvement_simple );
// same results should be achieved with unnormalized weights
// ( testing automatic weights normalization, i.e. using avg instead of sum )
node_lifecycle_->set_parameter(
rclcpp::Parameter(
"SmoothPath.cost_check_points",
std::vector<double>( {-0.05, 0.0, 1.0, -0.45, 0.0, 1.0} ) // x1, y1, weight1, x2, y2, weight2
) );
reloadParams( );
std::vector<Eigen::Vector3d> smoothed_path_scc_unnormalized;
EXPECT_TRUE( smoothPath( cusp_near_obstacle, smoothed_path_scc_unnormalized ) );
EXPECT_EQ( smoothed_path_scc, smoothed_path_scc_unnormalized );
////////////////////////////////////////
// compare also with extra careful cusp
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.w_cost", 0.0052 ) );
node_lifecycle_->set_parameter(
rclcpp::Parameter( "SmoothPath.w_cost_cusp_multiplier", 0.027 / 0.0052 ) );
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.cusp_zone_length", 2.5 ) );
// we need much more iterations here since it's a more complicated problem
// TODO( afrixs ): tune ceres optimizer to "converge" faster
// see http://ceres-solver.org/nnls_solving.html
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.optimizer.max_iterations", 1500 ) );
reloadParams( );
std::vector<Eigen::Vector3d> smoothed_path_scce;
EXPECT_TRUE( smoothPath( cusp_near_obstacle, smoothed_path_scce ) );
EXPECT_GT(
assessPathImprovement( upper_bound, smoothed_path_scce, mvmt_smoothness_criterion_ ),
0.0 );
double cost_avoidance_improvement_shifted_extra = assessPathImprovement(
cusp_near_obstacle,
smoothed_path_scce,
cost_avoidance_criterion );
double worst_cost_improvement_shifted_extra = assessWorstPoseImprovement(
cusp_near_obstacle,
smoothed_path_scce,
cost_avoidance_criterion );
RCLCPP_INFO(
rclcpp::get_logger(
"ceres_smoother" ), "Cost avoidance improvement ( cusp_shifted, scce ): %lf, %lf",
cost_avoidance_improvement_shifted_extra, worst_cost_improvement_shifted_extra );
EXPECT_NEAR( cost_avoidance_improvement_shifted_extra, 51.0, 1.0 );
EXPECT_GE( worst_cost_improvement_shifted_extra, 0.0 );
// resmooth extra careful cusp with same conditions ( higher max_iterations )
node_lifecycle_->set_parameter(
rclcpp::Parameter(
"SmoothPath.cost_check_points",
std::vector<double>( ) ) );
reloadParams( );
EXPECT_TRUE( smoothPath( cusp_near_obstacle, smoothed_path_ecc ) );
cost_avoidance_improvement_extra_careful_cusp = assessPathImprovement(
cusp_near_obstacle,
smoothed_path_ecc,
cost_avoidance_criterion );
worst_cost_improvement_extra_careful_cusp = assessWorstPoseImprovement(
cusp_near_obstacle,
smoothed_path_ecc,
cost_avoidance_criterion );
EXPECT_GT( cost_avoidance_improvement_extra_careful_cusp, 0.0 );
RCLCPP_INFO(
rclcpp::get_logger(
"ceres_smoother" ), "Cost avoidance improvement ( cusp_shifted, ecc ): %lf, %lf",
cost_avoidance_improvement_extra_careful_cusp, worst_cost_improvement_extra_careful_cusp );
EXPECT_NEAR( cost_avoidance_improvement_extra_careful_cusp, 48.5, 1.0 );
EXPECT_GT(
cost_avoidance_improvement_shifted_extra,
cost_avoidance_improvement_extra_careful_cusp );
// worst cost improvement is a bit lower but only by 5% so it's not a big deal
EXPECT_GE( worst_cost_improvement_shifted_extra, worst_cost_improvement_extra_careful_cusp - 6.0 );
SUCCEED( );
}
TEST_F( SmootherTest, testingDownsamplingUpsampling )
{
// path with a cusp
std::vector<Eigen::Vector3d> sharp_turn_90_then_reverse =
{{0, 0, 0},
{0.1, 0, 0},
{0.2, 0, 0},
{0.3, 0, 0},
{0.4, 0, 0},
{0.5, 0, 0},
{0.6, 0, M_PI / 4},
{0.6, -0.1, M_PI / 2},
{0.6, -0.2, M_PI / 2},
{0.6, -0.3, M_PI / 2},
{0.6, -0.4, M_PI / 2},
{0.6, -0.5, M_PI / 2},
{0.6, -0.6, M_PI / 2}
};
cusp_i_ = 6;
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.path_downsampling_factor", 2 ) );
// downsample only
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.path_upsampling_factor", 0 ) );
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.reversing_enabled", false ) );
reloadParams( );
std::vector<Eigen::Vector3d> smoothed_path_downsampled;
EXPECT_TRUE( smoothPath( sharp_turn_90_then_reverse, smoothed_path_downsampled ) );
// first two, last two and every 2nd pose between them
EXPECT_EQ( smoothed_path_downsampled.size( ), 8u );
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.reversing_enabled", true ) );
reloadParams( );
EXPECT_TRUE( smoothPath( sharp_turn_90_then_reverse, smoothed_path_downsampled ) );
// same but downsampling is reset on cusp
EXPECT_EQ( smoothed_path_downsampled.size( ), 9u );
// upsample to original size
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.path_upsampling_factor", 1 ) );
reloadParams( );
std::vector<Eigen::Vector3d> smoothed_path;
EXPECT_TRUE( smoothPath( sharp_turn_90_then_reverse, smoothed_path ) );
EXPECT_EQ( smoothed_path.size( ), sharp_turn_90_then_reverse.size( ) );
cusp_i_ = 4; // for downsampled path
int cusp_i_out = 6; // for upsampled path
QualityCriterion3 mvmt_smoothness_criterion_out =
[&cusp_i_out]( int i, const Eigen::Vector3d & prev_p, const Eigen::Vector3d & p,
const Eigen::Vector3d & next_p ) {
Eigen::Vector2d prev_mvmt = p.block<2, 1>( 0, 0 ) - prev_p.block<2, 1>( 0, 0 );
Eigen::Vector2d next_mvmt = next_p.block<2, 1>( 0, 0 ) - p.block<2, 1>( 0, 0 );
if ( i == cusp_i_out ) {
next_mvmt = -next_mvmt;
}
return ( next_mvmt - prev_mvmt ).norm( );
};
double smoothness_improvement = assessPathImprovement(
smoothed_path_downsampled, smoothed_path,
mvmt_smoothness_criterion_,
&mvmt_smoothness_criterion_out );
// more poses -> smoother path
EXPECT_GT( smoothness_improvement, 0.0 );
EXPECT_NEAR( smoothness_improvement, 65.7, 1.0 );
// upsample above original size
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.path_upsampling_factor", 2 ) );
reloadParams( );
EXPECT_TRUE( smoothPath( sharp_turn_90_then_reverse, smoothed_path ) );
// every pose except last produces 2 poses
EXPECT_EQ( smoothed_path.size( ), sharp_turn_90_then_reverse.size( ) * 2 - 1 );
cusp_i_out = 12; // for upsampled path
smoothness_improvement = assessPathImprovement(
smoothed_path_downsampled, smoothed_path,
mvmt_smoothness_criterion_,
&mvmt_smoothness_criterion_out );
// even more poses -> even smoother path
EXPECT_GT( smoothness_improvement, 0.0 );
EXPECT_NEAR( smoothness_improvement, 83.7, 1.0 );
}
TEST_F( SmootherTest, testingStartGoalOrientations )
{
std::vector<Eigen::Vector3d> sharp_turn_90 =
{{0, 0, 0},
{0.1, 0, 0},
{0.2, 0, 0},
{0.3, 0, M_PI / 4},
{0.3, 0.1, M_PI / 2},
{0.3, 0.2, M_PI / 2},
{0.3, 0.3, M_PI / 2}
};
// Keep start and goal orientations ( by default )
std::vector<Eigen::Vector3d> smoothed_path;
EXPECT_TRUE( smoothPath( sharp_turn_90, smoothed_path ) );
double mvmt_smoothness_improvement =
assessPathImprovement( sharp_turn_90, smoothed_path, mvmt_smoothness_criterion_ );
EXPECT_GT( mvmt_smoothness_improvement, 0.0 );
EXPECT_NEAR( mvmt_smoothness_improvement, 53.3, 1.0 );
// no change in orientations
EXPECT_NEAR( smoothed_path.front( )[2], 0, 0.001 );
EXPECT_NEAR( smoothed_path.back( )[2], M_PI / 2, 0.001 );
// Overwrite start/goal orientations
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.keep_start_orientation", false ) );
node_lifecycle_->set_parameter( rclcpp::Parameter( "SmoothPath.keep_goal_orientation", false ) );
reloadParams( );
sharp_turn_90[0][2] = M_PI; // forward/reverse of start pose should not matter
std::vector<Eigen::Vector3d> smoothed_path_sg_overwritten;
EXPECT_TRUE( smoothPath( sharp_turn_90, smoothed_path_sg_overwritten ) );
mvmt_smoothness_improvement =
assessPathImprovement( smoothed_path, smoothed_path_sg_overwritten, mvmt_smoothness_criterion_ );
EXPECT_GT( mvmt_smoothness_improvement, 0.0 );
EXPECT_NEAR( mvmt_smoothness_improvement, 98.3, 1.0 );
// orientations adjusted to follow the path
EXPECT_NEAR( smoothed_path_sg_overwritten.front( )[2], M_PI / 4, 0.1 );
EXPECT_NEAR( smoothed_path_sg_overwritten.back( )[2], M_PI / 4, 0.1 );
// test short paths
std::vector<Eigen::Vector3d> short_screwed_path =
{{0, 0, M_PI * 0.25},
{0.1, 0, -M_PI * 0.25}
};
std::vector<Eigen::Vector3d> adjusted_path;
EXPECT_TRUE( smoothPath( short_screwed_path, adjusted_path ) );
EXPECT_NEAR( adjusted_path.front( )[2], 0, 0.001 );
EXPECT_NEAR( adjusted_path.back( )[2], 0, 0.001 );
short_screwed_path[0][2] = M_PI * 0.75; // start is stronger than goal
EXPECT_TRUE( smoothPath( short_screwed_path, adjusted_path ) );
EXPECT_NEAR( adjusted_path.front( )[2], M_PI, 0.001 );
EXPECT_NEAR( adjusted_path.back( )[2], M_PI, 0.001 );
std::vector<Eigen::Vector3d> one_pose_path = {{0, 0, M_PI * 0.75}};
EXPECT_TRUE( smoothPath( one_pose_path, adjusted_path ) );
EXPECT_NEAR( adjusted_path.front( )[2], M_PI * 0.75, 0.001 );
}
TEST_F( SmootherTest, testingCostCheckPointsParamValidity )
{
node_lifecycle_->set_parameter(
rclcpp::Parameter(
"SmoothPath.cost_check_points",
std::vector<double>( ) ) );
reloadParams( );
node_lifecycle_->set_parameter(
rclcpp::Parameter(
"SmoothPath.cost_check_points",
std::vector<double>( {0, 0, 0, 0, 0, 0, 0, 0, 0} ) ) ); // multiple of 3 is ok
reloadParams( );
node_lifecycle_->set_parameter(
rclcpp::Parameter(
"SmoothPath.cost_check_points",
std::vector<double>( {0, 0} ) ) );
EXPECT_THROW( reloadParams( ), std::runtime_error );
}
TEST_F( SmootherTest, testingLinearSolverTypeParamValidity )
{
node_lifecycle_->set_parameter(
rclcpp::Parameter(
"SmoothPath.optimizer.linear_solver_type",
"SPARSE_NORMAL_CHOLESKY" ) );
reloadParams( );
node_lifecycle_->set_parameter(
rclcpp::Parameter(
"SmoothPath.optimizer.linear_solver_type",
"DENSE_QR" ) );
reloadParams( );
node_lifecycle_->set_parameter(
rclcpp::Parameter(
"SmoothPath.optimizer.linear_solver_type",
"INVALID_SOLVER" ) );
EXPECT_THROW( reloadParams( ), std::runtime_error );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2021 RoboTech Vision
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <string>
#include <memory>
#include <chrono>
#include <iostream>
#include <future>
#include <thread>
#include <algorithm>
#include <vector>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_constrained_smoother/smoother_cost_function.hpp"
28 class TestableSmootherCostFunction : nav2_constrained_smoother::SmootherCostFunction
{
public:
31 TestableSmootherCostFunction(
const Eigen::Vector2d & original_pos,
double next_to_last_length_ratio,
bool reversing,
const nav2_costmap_2d::Costmap2D * costmap,
const std::shared_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>> & costmap_interpolator,
const nav2_constrained_smoother::SmootherParams & params,
double costmap_weight )
: SmootherCostFunction(
original_pos, next_to_last_length_ratio, reversing,
costmap, costmap_interpolator,
params, costmap_weight )
{
}
46 inline double getCurvatureResidual(
const double & weight,
48 const Eigen::Vector2d & pt,
49 const Eigen::Vector2d & pt_next,
50 const Eigen::Vector2d & pt_prev ) const
{
double r = 0.0;
addCurvatureResidual<double>( weight, pt, pt_next, pt_prev, r );
return r;
}
};
58 class Test : public ::testing::Test
{
protected:
61 void SetUp( )
{
}
};
66 TEST_F( Test, testingCurvatureResidual )
{
nav2_costmap_2d::Costmap2D costmap;
TestableSmootherCostFunction fn(
Eigen::Vector2d( 1.0, 0.0 ), 1.0, false,
&costmap, std::shared_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>>( ),
nav2_constrained_smoother::SmootherParams( ), 0.0
);
// test for edge values
Eigen::Vector2d pt( 1.0, 0.0 );
Eigen::Vector2d pt_other( 0.0, 0.0 );
EXPECT_EQ( fn.getCurvatureResidual( 0.0, pt, pt_other, pt_other ), 0.0 );
nav2_constrained_smoother::SmootherParams params_no_min_turning_radius;
params_no_min_turning_radius.max_curvature = 1.0f / 0.0;
TestableSmootherCostFunction fn_no_min_turning_radius(
Eigen::Vector2d( 1.0, 0.0 ), 1.0, false,
&costmap, std::shared_ptr<ceres::BiCubicInterpolator<ceres::Grid2D<u_char>>>( ),
params_no_min_turning_radius, 0.0
);
EXPECT_EQ( fn_no_min_turning_radius.getCurvatureResidual( 1.0, pt, pt_other, pt_other ), 0.0 );
}
90 TEST_F( Test, testingUtils )
{
Eigen::Vector2d pt( 1.0, 0.0 );
Eigen::Vector2d pt_prev( 0.0, 0.0 );
Eigen::Vector2d pt_next( 0.0, 0.0 );
// test for intermediate values
auto center = nav2_constrained_smoother::arcCenter( pt_prev, pt, pt_next, false );
// although in this situation the center would be at ( 0.5, 0.0 ),
// cases where pt_prev == pt_next are very rare and thus unhandled
// during the smoothing points will be separated ( and thus made valid ) by smoothness cost anyways
EXPECT_EQ( center[0], std::numeric_limits<double>::infinity( ) );
EXPECT_EQ( center[1], std::numeric_limits<double>::infinity( ) );
auto tangent =
nav2_constrained_smoother::tangentDir( pt_prev, pt, pt_next, false ).normalized( );
EXPECT_NEAR( tangent[0], 0, 1e-10 );
EXPECT_NEAR( std::abs( tangent[1] ), 1, 1e-10 );
// no rotation when mid point is a cusp
tangent = nav2_constrained_smoother::tangentDir( pt_prev, pt, pt_next, true ).normalized( );
EXPECT_NEAR( std::abs( tangent[0] ), 1, 1e-10 );
EXPECT_NEAR( tangent[1], 0, 1e-10 );
pt_prev[0] = -1.0;
// rotation is mathematically invalid, picking direction of a shorter segment
tangent = nav2_constrained_smoother::tangentDir( pt_prev, pt, pt_next, true ).normalized( );
EXPECT_NEAR( std::abs( tangent[0] ), 1, 1e-10 );
EXPECT_NEAR( tangent[1], 0, 1e-10 );
pt_prev[0] = 0.0;
pt_next[0] = -1.0;
// rotation is mathematically invalid, picking direction of a shorter segment
tangent = nav2_constrained_smoother::tangentDir( pt_prev, pt, pt_next, true ).normalized( );
EXPECT_NEAR( std::abs( tangent[0] ), 1, 1e-10 );
EXPECT_NEAR( tangent[1], 0, 1e-10 );
}
128 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_CONTROLLER__CONTROLLER_SERVER_HPP_
#define NAV2_CONTROLLER__CONTROLLER_SERVER_HPP_
#include <memory>
#include <string>
#include <thread>
#include <unordered_map>
#include <vector>
#include <mutex>
#include "nav2_core/controller.hpp"
#include "nav2_core/progress_checker.hpp"
#include "nav2_core/goal_checker.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "tf2_ros/transform_listener.h"
#include "nav2_msgs/action/follow_path.hpp"
#include "nav2_msgs/msg/speed_limit.hpp"
#include "nav_2d_utils/odom_subscriber.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/simple_action_server.hpp"
#include "nav2_util/robot_utils.hpp"
#include "pluginlib/class_loader.hpp"
#include "pluginlib/class_list_macros.hpp"
namespace nav2_controller
{
42 class ProgressChecker;
/**
* @class nav2_controller::ControllerServer
* @brief This class hosts variety of plugins of different algorithms to
* complete control tasks from the exposed FollowPath action server.
*/
48 class ControllerServer : public nav2_util::LifecycleNode
{
public:
using ControllerMap = std::unordered_map<std::string, nav2_core::Controller::Ptr>;
using GoalCheckerMap = std::unordered_map<std::string, nav2_core::GoalChecker::Ptr>;
/**
* @brief Constructor for nav2_controller::ControllerServer
* @param options Additional options to control creation of the node.
*/
explicit ControllerServer( const rclcpp::NodeOptions & options = rclcpp::NodeOptions( ) );
/**
* @brief Destructor for nav2_controller::ControllerServer
*/
~ControllerServer( );
protected:
/**
* @brief Configures controller parameters and member variables
*
* Configures controller plugin and costmap; Initialize odom subscriber,
* velocity publisher and follow path action server.
* @param state LifeCycle Node's state
* @return Success or Failure
* @throw pluginlib::PluginlibException When failed to initialize controller
* plugin
*/
nav2_util::CallbackReturn on_configure( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Activates member variables
*
* Activates controller, costmap, velocity publisher and follow path action
* server
* @param state LifeCycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_activate( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Deactivates member variables
*
* Deactivates follow path action server, controller, costmap and velocity
* publisher. Before calling deactivate state, velocity is being set to zero.
* @param state LifeCycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Calls clean up states and resets member variables.
*
* Controller and costmap clean up state is called, and resets rest of the
* variables
* @param state LifeCycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Called when in Shutdown state
* @param state LifeCycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_shutdown( const rclcpp_lifecycle::State & state ) override;
using Action = nav2_msgs::action::FollowPath;
using ActionServer = nav2_util::SimpleActionServer<Action>;
// Our action server implements the FollowPath action
std::unique_ptr<ActionServer> action_server_;
/**
* @brief FollowPath action server callback. Handles action server updates and
* spins server until goal is reached
*
* Provides global path to controller received from action client. Twist
* velocities for the robot are calculated and published using controller at
* the specified rate till the goal is reached.
* @throw nav2_core::PlannerException
*/
void computeControl( );
/**
* @brief Find the valid controller ID name for the given request
*
* @param c_name The requested controller name
* @param name Reference to the name to use for control if any valid available
* @return bool Whether it found a valid controller to use
*/
bool findControllerId( const std::string & c_name, std::string & name );
/**
* @brief Find the valid goal checker ID name for the specified parameter
*
* @param c_name The goal checker name
* @param name Reference to the name to use for goal checking if any valid available
* @return bool Whether it found a valid goal checker to use
*/
bool findGoalCheckerId( const std::string & c_name, std::string & name );
/**
* @brief Assigns path to controller
* @param path Path received from action server
*/
void setPlannerPath( const nav_msgs::msg::Path & path );
/**
* @brief Calculates velocity and publishes to "cmd_vel" topic
*/
void computeAndPublishVelocity( );
/**
* @brief Calls setPlannerPath method with an updated path received from
* action server
*/
void updateGlobalPath( );
/**
* @brief Calls velocity publisher to publish the velocity on "cmd_vel" topic
* @param velocity Twist velocity to be published
*/
void publishVelocity( const geometry_msgs::msg::TwistStamped & velocity );
/**
* @brief Calls velocity publisher to publish zero velocity
*/
void publishZeroVelocity( );
/**
* @brief Checks if goal is reached
* @return true or false
*/
bool isGoalReached( );
/**
* @brief Obtain current pose of the robot
* @param pose To store current pose of the robot
* @return true if able to obtain current pose of the robot, else false
*/
bool getRobotPose( geometry_msgs::msg::PoseStamped & pose );
/**
* @brief get the thresholded velocity
* @param velocity The current velocity from odometry
* @param threshold The minimum velocity to return non-zero
* @return double velocity value
*/
double getThresholdedVelocity( double velocity, double threshold )
{
return ( std::abs( velocity ) > threshold ) ? velocity : 0.0;
}
/**
* @brief get the thresholded Twist
* @param Twist The current Twist from odometry
* @return Twist Twist after thresholds applied
*/
196 nav_2d_msgs::msg::Twist2D getThresholdedTwist( const nav_2d_msgs::msg::Twist2D & twist )
{
nav_2d_msgs::msg::Twist2D twist_thresh;
twist_thresh.x = getThresholdedVelocity( twist.x, min_x_velocity_threshold_ );
twist_thresh.y = getThresholdedVelocity( twist.y, min_y_velocity_threshold_ );
twist_thresh.theta = getThresholdedVelocity( twist.theta, min_theta_velocity_threshold_ );
return twist_thresh;
}
/**
* @brief Callback executed when a parameter change is detected
* @param event ParameterEvent message
*/
rcl_interfaces::msg::SetParametersResult
210 dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters );
// Dynamic parameters handler
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
std::mutex dynamic_params_lock_;
// The controller needs a costmap node
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
std::unique_ptr<nav2_util::NodeThread> costmap_thread_;
// Publishers and subscribers
std::unique_ptr<nav_2d_utils::OdomSubscriber> odom_sub_;
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_publisher_;
rclcpp::Subscription<nav2_msgs::msg::SpeedLimit>::SharedPtr speed_limit_sub_;
// Progress Checker Plugin
pluginlib::ClassLoader<nav2_core::ProgressChecker> progress_checker_loader_;
nav2_core::ProgressChecker::Ptr progress_checker_;
std::string default_progress_checker_id_;
std::string default_progress_checker_type_;
std::string progress_checker_id_;
std::string progress_checker_type_;
// Goal Checker Plugin
pluginlib::ClassLoader<nav2_core::GoalChecker> goal_checker_loader_;
GoalCheckerMap goal_checkers_;
std::vector<std::string> default_goal_checker_ids_;
std::vector<std::string> default_goal_checker_types_;
std::vector<std::string> goal_checker_ids_;
std::vector<std::string> goal_checker_types_;
std::string goal_checker_ids_concat_, current_goal_checker_;
// Controller Plugins
pluginlib::ClassLoader<nav2_core::Controller> lp_loader_;
ControllerMap controllers_;
std::vector<std::string> default_ids_;
std::vector<std::string> default_types_;
std::vector<std::string> controller_ids_;
std::vector<std::string> controller_types_;
std::string controller_ids_concat_, current_controller_;
double controller_frequency_;
double min_x_velocity_threshold_;
double min_y_velocity_threshold_;
double min_theta_velocity_threshold_;
double failure_tolerance_;
// Whether we've published the single controller warning yet
geometry_msgs::msg::PoseStamped end_pose_;
// Last time the controller generated a valid command
rclcpp::Time last_valid_cmd_time_;
// Current path container
nav_msgs::msg::Path current_path_;
private:
/**
* @brief Callback for speed limiting messages
* @param msg Shared pointer to nav2_msgs::msg::SpeedLimit
*/
272 void speedLimitCallback( const nav2_msgs::msg::SpeedLimit::SharedPtr msg );
};
} // namespace nav2_controller
#endif // NAV2_CONTROLLER__CONTROLLER_SERVER_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_
#define NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_
#include <memory>
#include <string>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "nav2_core/goal_checker.hpp"
#include "rcl_interfaces/msg/set_parameters_result.hpp"
namespace nav2_controller
{
/**
* @class SimpleGoalChecker
* @brief Goal Checker plugin that only checks the position difference
*
* This class can be stateful if the stateful parameter is set to true ( which it is by default ).
* This means that the goal checker will not check if the xy position matches again once it is found to be true.
*/
57 class SimpleGoalChecker : public nav2_core::GoalChecker
{
public:
60 SimpleGoalChecker( );
// Standard GoalChecker Interface
void initialize(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
const std::string & plugin_name,
const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros ) override;
void reset( ) override;
bool isGoalReached(
const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
const geometry_msgs::msg::Twist & velocity ) override;
bool getTolerances(
geometry_msgs::msg::Pose & pose_tolerance,
geometry_msgs::msg::Twist & vel_tolerance ) override;
protected:
double xy_goal_tolerance_, yaw_goal_tolerance_;
bool stateful_, check_xy_;
// Cached squared xy_goal_tolerance_
double xy_goal_tolerance_sq_;
// Dynamic parameters handler
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
std::string plugin_name_;
/**
* @brief Callback executed when a paramter change is detected
* @param parameters list of changed parameters
*/
rcl_interfaces::msg::SetParametersResult
dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters );
};
} // namespace nav2_controller
#endif // NAV2_CONTROLLER__PLUGINS__SIMPLE_GOAL_CHECKER_HPP_
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_
#define NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_
#include <string>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "nav2_core/progress_checker.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose2_d.hpp"
namespace nav2_controller
{
/**
* @class SimpleProgressChecker
* @brief This plugin is used to check the position of the robot to make sure
* that it is actually progressing towards a goal.
*/
34 class SimpleProgressChecker : public nav2_core::ProgressChecker
{
public:
void initialize(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
const std::string & plugin_name ) override;
bool check( geometry_msgs::msg::PoseStamped & current_pose ) override;
void reset( ) override;
protected:
/**
* @brief Calculates robots movement from baseline pose
* @param pose Current pose of the robot
* @return true, if movement is greater than radius_, or false
*/
bool is_robot_moved_enough( const geometry_msgs::msg::Pose2D & pose );
/**
* @brief Resets baseline pose with the current pose of the robot
* @param pose Current pose of the robot
*/
void reset_baseline_pose( const geometry_msgs::msg::Pose2D & pose );
rclcpp::Clock::SharedPtr clock_;
double radius_;
rclcpp::Duration time_allowance_{0, 0};
geometry_msgs::msg::Pose2D baseline_pose_;
rclcpp::Time baseline_time_;
bool baseline_pose_set_{false};
// Dynamic parameters handler
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
std::string plugin_name_;
/**
* @brief Callback executed when a paramter change is detected
* @param parameters list of changed parameters
*/
rcl_interfaces::msg::SetParametersResult
74 dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters );
};
} // namespace nav2_controller
#endif // NAV2_CONTROLLER__PLUGINS__SIMPLE_PROGRESS_CHECKER_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_
#define NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_
#include <memory>
#include <string>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "nav2_controller/plugins/simple_goal_checker.hpp"
namespace nav2_controller
{
/**
* @class StoppedGoalChecker
* @brief Goal Checker plugin that checks the position difference and velocity
*/
53 class StoppedGoalChecker : public SimpleGoalChecker
{
public:
56 StoppedGoalChecker( );
// Standard GoalChecker Interface
void initialize(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
const std::string & plugin_name,
const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros ) override;
bool isGoalReached(
const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
const geometry_msgs::msg::Twist & velocity ) override;
bool getTolerances(
geometry_msgs::msg::Pose & pose_tolerance,
geometry_msgs::msg::Twist & vel_tolerance ) override;
protected:
double rot_stopped_velocity_, trans_stopped_velocity_;
// Dynamic parameters handler
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
std::string plugin_name_;
/**
* @brief Callback executed when a paramter change is detected
* @param parameters list of changed parameters
*/
rcl_interfaces::msg::SetParametersResult
dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters );
};
} // namespace nav2_controller
#endif // NAV2_CONTROLLER__PLUGINS__STOPPED_GOAL_CHECKER_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <memory>
#include <string>
#include <limits>
#include <vector>
#include "nav2_controller/plugins/simple_goal_checker.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "angles/angles.h"
#include "nav2_util/node_utils.hpp"
#include "nav2_util/geometry_utils.hpp"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpedantic"
#include "tf2/utils.h"
#pragma GCC diagnostic pop
using rcl_interfaces::msg::ParameterType;
using std::placeholders::_1;
namespace nav2_controller
{
55 SimpleGoalChecker::SimpleGoalChecker( )
: xy_goal_tolerance_( 0.25 ),
yaw_goal_tolerance_( 0.25 ),
stateful_( true ),
check_xy_( true ),
xy_goal_tolerance_sq_( 0.0625 )
{
}
64 void SimpleGoalChecker::initialize(
65 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
66 const std::string & plugin_name,
67 const std::shared_ptr<nav2_costmap_2d::Costmap2DROS>/*costmap_ros*/ )
{
plugin_name_ = plugin_name;
auto node = parent.lock( );
nav2_util::declare_parameter_if_not_declared(
node,
plugin_name + ".xy_goal_tolerance", rclcpp::ParameterValue( 0.25 ) );
nav2_util::declare_parameter_if_not_declared(
node,
plugin_name + ".yaw_goal_tolerance", rclcpp::ParameterValue( 0.25 ) );
nav2_util::declare_parameter_if_not_declared(
node,
plugin_name + ".stateful", rclcpp::ParameterValue( true ) );
node->get_parameter( plugin_name + ".xy_goal_tolerance", xy_goal_tolerance_ );
node->get_parameter( plugin_name + ".yaw_goal_tolerance", yaw_goal_tolerance_ );
node->get_parameter( plugin_name + ".stateful", stateful_ );
xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
// Add callback for dynamic parameters
dyn_params_handler_ = node->add_on_set_parameters_callback(
std::bind( &SimpleGoalChecker::dynamicParametersCallback, this, _1 ) );
}
93 void SimpleGoalChecker::reset( )
{
check_xy_ = true;
}
98 bool SimpleGoalChecker::isGoalReached(
99 const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
100 const geometry_msgs::msg::Twist & )
{
if ( check_xy_ ) {
double dx = query_pose.position.x - goal_pose.position.x,
dy = query_pose.position.y - goal_pose.position.y;
if ( dx * dx + dy * dy > xy_goal_tolerance_sq_ ) {
return false;
}
// We are within the window
// If we are stateful, change the state.
if ( stateful_ ) {
check_xy_ = false;
}
}
double dyaw = angles::shortest_angular_distance(
tf2::getYaw( query_pose.orientation ),
tf2::getYaw( goal_pose.orientation ) );
return fabs( dyaw ) < yaw_goal_tolerance_;
}
120 bool SimpleGoalChecker::getTolerances(
121 geometry_msgs::msg::Pose & pose_tolerance,
122 geometry_msgs::msg::Twist & vel_tolerance )
{
double invalid_field = std::numeric_limits<double>::lowest( );
pose_tolerance.position.x = xy_goal_tolerance_;
pose_tolerance.position.y = xy_goal_tolerance_;
pose_tolerance.position.z = invalid_field;
pose_tolerance.orientation =
nav2_util::geometry_utils::orientationAroundZAxis( yaw_goal_tolerance_ );
vel_tolerance.linear.x = invalid_field;
vel_tolerance.linear.y = invalid_field;
vel_tolerance.linear.z = invalid_field;
vel_tolerance.angular.x = invalid_field;
vel_tolerance.angular.y = invalid_field;
vel_tolerance.angular.z = invalid_field;
return true;
}
rcl_interfaces::msg::SetParametersResult
144 SimpleGoalChecker::dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters )
{
rcl_interfaces::msg::SetParametersResult result;
for ( auto & parameter : parameters ) {
const auto & type = parameter.get_type( );
const auto & name = parameter.get_name( );
if ( type == ParameterType::PARAMETER_DOUBLE ) {
if ( name == plugin_name_ + ".xy_goal_tolerance" ) {
xy_goal_tolerance_ = parameter.as_double( );
xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
} else if ( name == plugin_name_ + ".yaw_goal_tolerance" ) {
yaw_goal_tolerance_ = parameter.as_double( );
}
} else if ( type == ParameterType::PARAMETER_BOOL ) {
if ( name == plugin_name_ + ".stateful" ) {
stateful_ = parameter.as_bool( );
}
}
}
result.successful = true;
return result;
}
} // namespace nav2_controller
170 PLUGINLIB_EXPORT_CLASS( nav2_controller::SimpleGoalChecker, nav2_core::GoalChecker )
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "nav2_controller/plugins/simple_progress_checker.hpp"
#include <cmath>
#include <string>
#include <memory>
#include <vector>
#include "nav2_core/exceptions.hpp"
#include "nav_2d_utils/conversions.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose2_d.hpp"
#include "nav2_util/node_utils.hpp"
#include "pluginlib/class_list_macros.hpp"
using rcl_interfaces::msg::ParameterType;
using std::placeholders::_1;
namespace nav2_controller
{
32 static double pose_distance( const geometry_msgs::msg::Pose2D &, const geometry_msgs::msg::Pose2D & );
34 void SimpleProgressChecker::initialize(
35 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
36 const std::string & plugin_name )
{
plugin_name_ = plugin_name;
auto node = parent.lock( );
clock_ = node->get_clock( );
nav2_util::declare_parameter_if_not_declared(
node, plugin_name + ".required_movement_radius", rclcpp::ParameterValue( 0.5 ) );
nav2_util::declare_parameter_if_not_declared(
node, plugin_name + ".movement_time_allowance", rclcpp::ParameterValue( 10.0 ) );
// Scale is set to 0 by default, so if it was not set otherwise, set to 0
node->get_parameter_or( plugin_name + ".required_movement_radius", radius_, 0.5 );
double time_allowance_param = 0.0;
node->get_parameter_or( plugin_name + ".movement_time_allowance", time_allowance_param, 10.0 );
time_allowance_ = rclcpp::Duration::from_seconds( time_allowance_param );
// Add callback for dynamic parameters
dyn_params_handler_ = node->add_on_set_parameters_callback(
std::bind( &SimpleProgressChecker::dynamicParametersCallback, this, _1 ) );
}
58 bool SimpleProgressChecker::check( geometry_msgs::msg::PoseStamped & current_pose )
{
// relies on short circuit evaluation to not call is_robot_moved_enough if
// baseline_pose is not set.
geometry_msgs::msg::Pose2D current_pose2d;
current_pose2d = nav_2d_utils::poseToPose2D( current_pose.pose );
if ( ( !baseline_pose_set_ ) || ( is_robot_moved_enough( current_pose2d ) ) ) {
reset_baseline_pose( current_pose2d );
return true;
}
return !( ( clock_->now( ) - baseline_time_ ) > time_allowance_ );
}
72 void SimpleProgressChecker::reset( )
{
baseline_pose_set_ = false;
}
77 void SimpleProgressChecker::reset_baseline_pose( const geometry_msgs::msg::Pose2D & pose )
{
baseline_pose_ = pose;
baseline_time_ = clock_->now( );
baseline_pose_set_ = true;
}
84 bool SimpleProgressChecker::is_robot_moved_enough( const geometry_msgs::msg::Pose2D & pose )
{
return pose_distance( pose, baseline_pose_ ) > radius_;
}
89 static double pose_distance(
const geometry_msgs::msg::Pose2D & pose1,
const geometry_msgs::msg::Pose2D & pose2 )
{
double dx = pose1.x - pose2.x;
double dy = pose1.y - pose2.y;
return std::hypot( dx, dy );
}
rcl_interfaces::msg::SetParametersResult
100 SimpleProgressChecker::dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters )
{
rcl_interfaces::msg::SetParametersResult result;
for ( auto parameter : parameters ) {
const auto & type = parameter.get_type( );
const auto & name = parameter.get_name( );
if ( type == ParameterType::PARAMETER_DOUBLE ) {
if ( name == plugin_name_ + ".required_movement_radius" ) {
radius_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".movement_time_allowance" ) {
time_allowance_ = rclcpp::Duration::from_seconds( parameter.as_double( ) );
}
}
}
result.successful = true;
return result;
}
} // namespace nav2_controller
121 PLUGINLIB_EXPORT_CLASS( nav2_controller::SimpleProgressChecker, nav2_core::ProgressChecker )
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <cmath>
#include <string>
#include <memory>
#include <limits>
#include <vector>
#include "nav2_controller/plugins/stopped_goal_checker.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "nav2_util/node_utils.hpp"
using std::hypot;
using std::fabs;
using rcl_interfaces::msg::ParameterType;
using std::placeholders::_1;
namespace nav2_controller
{
53 StoppedGoalChecker::StoppedGoalChecker( )
: SimpleGoalChecker( ), rot_stopped_velocity_( 0.25 ), trans_stopped_velocity_( 0.25 )
{
}
58 void StoppedGoalChecker::initialize(
59 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
60 const std::string & plugin_name,
61 const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros )
{
plugin_name_ = plugin_name;
SimpleGoalChecker::initialize( parent, plugin_name, costmap_ros );
auto node = parent.lock( );
nav2_util::declare_parameter_if_not_declared(
node,
plugin_name + ".rot_stopped_velocity", rclcpp::ParameterValue( 0.25 ) );
nav2_util::declare_parameter_if_not_declared(
node,
plugin_name + ".trans_stopped_velocity", rclcpp::ParameterValue( 0.25 ) );
node->get_parameter( plugin_name + ".rot_stopped_velocity", rot_stopped_velocity_ );
node->get_parameter( plugin_name + ".trans_stopped_velocity", trans_stopped_velocity_ );
// Add callback for dynamic parameters
dyn_params_handler_ = node->add_on_set_parameters_callback(
std::bind( &StoppedGoalChecker::dynamicParametersCallback, this, _1 ) );
}
83 bool StoppedGoalChecker::isGoalReached(
84 const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
85 const geometry_msgs::msg::Twist & velocity )
{
bool ret = SimpleGoalChecker::isGoalReached( query_pose, goal_pose, velocity );
if ( !ret ) {
return ret;
}
return fabs( velocity.angular.z ) <= rot_stopped_velocity_ &&
hypot( velocity.linear.x, velocity.linear.y ) <= trans_stopped_velocity_;
}
96 bool StoppedGoalChecker::getTolerances(
97 geometry_msgs::msg::Pose & pose_tolerance,
98 geometry_msgs::msg::Twist & vel_tolerance )
{
double invalid_field = std::numeric_limits<double>::lowest( );
// populate the poses
bool rtn = SimpleGoalChecker::getTolerances( pose_tolerance, vel_tolerance );
// override the velocities
vel_tolerance.linear.x = trans_stopped_velocity_;
vel_tolerance.linear.y = trans_stopped_velocity_;
vel_tolerance.linear.z = invalid_field;
vel_tolerance.angular.x = invalid_field;
vel_tolerance.angular.y = invalid_field;
vel_tolerance.angular.z = rot_stopped_velocity_;
return true && rtn;
}
rcl_interfaces::msg::SetParametersResult
118 StoppedGoalChecker::dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters )
{
rcl_interfaces::msg::SetParametersResult result;
for ( auto parameter : parameters ) {
const auto & type = parameter.get_type( );
const auto & name = parameter.get_name( );
if ( type == ParameterType::PARAMETER_DOUBLE ) {
if ( name == plugin_name_ + ".rot_stopped_velocity" ) {
rot_stopped_velocity_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".trans_stopped_velocity" ) {
trans_stopped_velocity_ = parameter.as_double( );
}
}
}
result.successful = true;
return result;
}
} // namespace nav2_controller
139 PLUGINLIB_EXPORT_CLASS( nav2_controller::StoppedGoalChecker, nav2_core::GoalChecker )
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <memory>
#include <string>
#include "gtest/gtest.h"
#include "nav2_controller/plugins/simple_goal_checker.hpp"
#include "nav2_controller/plugins/stopped_goal_checker.hpp"
#include "nav_2d_utils/conversions.hpp"
#include "nav2_util/lifecycle_node.hpp"
using nav2_controller::SimpleGoalChecker;
using nav2_controller::StoppedGoalChecker;
47 void checkMacro(
nav2_core::GoalChecker & gc,
double x0, double y0, double theta0,
double x1, double y1, double theta1,
double xv, double yv, double thetav,
bool expected_result )
{
gc.reset( );
geometry_msgs::msg::Pose2D pose0, pose1;
pose0.x = x0;
pose0.y = y0;
pose0.theta = theta0;
pose1.x = x1;
pose1.y = y1;
pose1.theta = theta1;
nav_2d_msgs::msg::Twist2D v;
v.x = xv;
v.y = yv;
v.theta = thetav;
if ( expected_result ) {
EXPECT_TRUE(
gc.isGoalReached(
nav_2d_utils::pose2DToPose( pose0 ),
nav_2d_utils::pose2DToPose( pose1 ), nav_2d_utils::twist2Dto3D( v ) ) );
} else {
EXPECT_FALSE(
gc.isGoalReached(
nav_2d_utils::pose2DToPose( pose0 ),
nav_2d_utils::pose2DToPose( pose1 ), nav_2d_utils::twist2Dto3D( v ) ) );
}
}
79 void sameResult(
nav2_core::GoalChecker & gc0, nav2_core::GoalChecker & gc1,
double x0, double y0, double theta0,
double x1, double y1, double theta1,
double xv, double yv, double thetav,
bool expected_result )
{
checkMacro( gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, expected_result );
checkMacro( gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, expected_result );
}
90 void trueFalse(
nav2_core::GoalChecker & gc0, nav2_core::GoalChecker & gc1,
double x0, double y0, double theta0,
double x1, double y1, double theta1,
double xv, double yv, double thetav )
{
checkMacro( gc0, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, true );
checkMacro( gc1, x0, y0, theta0, x1, y1, theta1, xv, yv, thetav, false );
}
99 class TestLifecycleNode : public nav2_util::LifecycleNode
{
public:
102 explicit TestLifecycleNode( const std::string & name )
: nav2_util::LifecycleNode( name )
{
}
107 nav2_util::CallbackReturn on_configure( const rclcpp_lifecycle::State & )
{
return nav2_util::CallbackReturn::SUCCESS;
}
112 nav2_util::CallbackReturn on_activate( const rclcpp_lifecycle::State & )
{
return nav2_util::CallbackReturn::SUCCESS;
}
117 nav2_util::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & )
{
return nav2_util::CallbackReturn::SUCCESS;
}
122 nav2_util::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & )
{
return nav2_util::CallbackReturn::SUCCESS;
}
127 nav2_util::CallbackReturn onShutdown( const rclcpp_lifecycle::State & )
{
return nav2_util::CallbackReturn::SUCCESS;
}
132 nav2_util::CallbackReturn onError( const rclcpp_lifecycle::State & )
{
return nav2_util::CallbackReturn::SUCCESS;
}
};
138 TEST( VelocityIterator, goal_checker_reset )
{
auto x = std::make_shared<TestLifecycleNode>( "goal_checker" );
nav2_core::GoalChecker * gc = new SimpleGoalChecker;
gc->reset( );
delete gc;
EXPECT_TRUE( true );
}
148 TEST( VelocityIterator, two_checks )
{
auto x = std::make_shared<TestLifecycleNode>( "goal_checker" );
SimpleGoalChecker gc;
StoppedGoalChecker sgc;
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "test_costmap" );
gc.initialize( x, "nav2_controller", costmap );
sgc.initialize( x, "nav2_controller", costmap );
sameResult( gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 0, true );
sameResult( gc, sgc, 0, 0, 0, 1, 0, 0, 0, 0, 0, false );
sameResult( gc, sgc, 0, 0, 0, 0, 1, 0, 0, 0, 0, false );
sameResult( gc, sgc, 0, 0, 0, 0, 0, 1, 0, 0, 0, false );
sameResult( gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 0, 0, 0, true );
trueFalse( gc, sgc, 0, 0, 3.14, 0, 0, -3.14, 1, 0, 0 );
trueFalse( gc, sgc, 0, 0, 0, 0, 0, 0, 1, 0, 0 );
trueFalse( gc, sgc, 0, 0, 0, 0, 0, 0, 0, 1, 0 );
trueFalse( gc, sgc, 0, 0, 0, 0, 0, 0, 0, 0, 1 );
}
169 TEST( StoppedGoalChecker, get_tol_and_dynamic_params )
{
auto x = std::make_shared<TestLifecycleNode>( "goal_checker" );
SimpleGoalChecker gc;
StoppedGoalChecker sgc;
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "test_costmap" );
sgc.initialize( x, "test", costmap );
gc.initialize( x, "test2", costmap );
geometry_msgs::msg::Pose pose_tol;
geometry_msgs::msg::Twist vel_tol;
// Test stopped goal checker's tolerance API
EXPECT_TRUE( sgc.getTolerances( pose_tol, vel_tol ) );
EXPECT_EQ( vel_tol.linear.x, 0.25 );
EXPECT_EQ( vel_tol.linear.y, 0.25 );
EXPECT_EQ( vel_tol.angular.z, 0.25 );
// Test Stopped goal checker's dynamic parameters
auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
x->get_node_base_interface( ), x->get_node_topics_interface( ),
x->get_node_graph_interface( ),
x->get_node_services_interface( ) );
auto results = rec_param->set_parameters_atomically(
{rclcpp::Parameter( "test.rot_stopped_velocity", 100.0 ),
rclcpp::Parameter( "test.trans_stopped_velocity", 100.0 )} );
rclcpp::spin_until_future_complete(
x->get_node_base_interface( ),
results );
EXPECT_EQ( x->get_parameter( "test.rot_stopped_velocity" ).as_double( ), 100.0 );
EXPECT_EQ( x->get_parameter( "test.trans_stopped_velocity" ).as_double( ), 100.0 );
// Test normal goal checker's dynamic parameters
results = rec_param->set_parameters_atomically(
{rclcpp::Parameter( "test2.xy_goal_tolerance", 200.0 ),
rclcpp::Parameter( "test2.yaw_goal_tolerance", 200.0 ),
rclcpp::Parameter( "test2.stateful", true )} );
rclcpp::spin_until_future_complete(
x->get_node_base_interface( ),
results );
EXPECT_EQ( x->get_parameter( "test2.xy_goal_tolerance" ).as_double( ), 200.0 );
EXPECT_EQ( x->get_parameter( "test2.yaw_goal_tolerance" ).as_double( ), 200.0 );
EXPECT_EQ( x->get_parameter( "test2.stateful" ).as_bool( ), true );
// Test the dynamic parameters impacted the tolerances
EXPECT_TRUE( sgc.getTolerances( pose_tol, vel_tol ) );
EXPECT_EQ( vel_tol.linear.x, 100.0 );
EXPECT_EQ( vel_tol.linear.y, 100.0 );
EXPECT_EQ( vel_tol.angular.z, 100.0 );
EXPECT_TRUE( gc.getTolerances( pose_tol, vel_tol ) );
EXPECT_EQ( pose_tol.position.x, 200.0 );
EXPECT_EQ( pose_tol.position.y, 200.0 );
}
230 int main( int argc, char ** argv )
{
rclcpp::init( argc, argv );
testing::InitGoogleTest( &argc, argv );
return RUN_ALL_TESTS( );
}
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <memory>
#include <string>
#include "gtest/gtest.h"
#include "nav2_controller/plugins/simple_progress_checker.hpp"
#include "nav_2d_utils/conversions.hpp"
#include "nav2_util/lifecycle_node.hpp"
using nav2_controller::SimpleProgressChecker;
45 class TestLifecycleNode : public nav2_util::LifecycleNode
{
public:
48 explicit TestLifecycleNode( const std::string & name )
: nav2_util::LifecycleNode( name )
{
}
53 nav2_util::CallbackReturn on_configure( const rclcpp_lifecycle::State & )
{
return nav2_util::CallbackReturn::SUCCESS;
}
58 nav2_util::CallbackReturn on_activate( const rclcpp_lifecycle::State & )
{
return nav2_util::CallbackReturn::SUCCESS;
}
63 nav2_util::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & )
{
return nav2_util::CallbackReturn::SUCCESS;
}
68 nav2_util::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & )
{
return nav2_util::CallbackReturn::SUCCESS;
}
73 nav2_util::CallbackReturn onShutdown( const rclcpp_lifecycle::State & )
{
return nav2_util::CallbackReturn::SUCCESS;
}
78 nav2_util::CallbackReturn onError( const rclcpp_lifecycle::State & )
{
return nav2_util::CallbackReturn::SUCCESS;
}
};
84 void checkMacro(
nav2_core::ProgressChecker & pc,
double x0, double y0,
double x1, double y1,
int delay,
bool expected_result )
{
pc.reset( );
geometry_msgs::msg::PoseStamped pose0, pose1;
pose0.pose.position.x = x0;
pose0.pose.position.y = y0;
pose1.pose.position.x = x1;
pose1.pose.position.y = y1;
EXPECT_TRUE( pc.check( pose0 ) );
rclcpp::sleep_for( std::chrono::seconds( delay ) );
if ( expected_result ) {
EXPECT_TRUE( pc.check( pose1 ) );
} else {
EXPECT_FALSE( pc.check( pose1 ) );
}
}
106 TEST( SimpleProgressChecker, progress_checker_reset )
{
auto x = std::make_shared<TestLifecycleNode>( "progress_checker" );
nav2_core::ProgressChecker * pc = new SimpleProgressChecker;
pc->reset( );
delete pc;
EXPECT_TRUE( true );
}
116 TEST( SimpleProgressChecker, unit_tests )
{
auto x = std::make_shared<TestLifecycleNode>( "progress_checker" );
SimpleProgressChecker pc;
pc.initialize( x, "nav2_controller" );
checkMacro( pc, 0, 0, 0, 0, 1, true );
checkMacro( pc, 0, 0, 1, 0, 1, true );
checkMacro( pc, 0, 0, 0, 1, 1, true );
checkMacro( pc, 0, 0, 1, 0, 11, true );
checkMacro( pc, 0, 0, 0, 1, 11, true );
checkMacro( pc, 0, 0, 0, 0, 11, false );
}
130 int main( int argc, char ** argv )
{
rclcpp::init( argc, argv );
testing::InitGoogleTest( &argc, argv );
return RUN_ALL_TESTS( );
}
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <chrono>
#include <vector>
#include <memory>
#include <string>
#include <utility>
#include <limits>
#include "nav2_core/exceptions.hpp"
#include "nav_2d_utils/conversions.hpp"
#include "nav_2d_utils/tf_help.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_controller/controller_server.hpp"
using namespace std::chrono_literals;
using rcl_interfaces::msg::ParameterType;
using std::placeholders::_1;
namespace nav2_controller
{
36 ControllerServer::ControllerServer( const rclcpp::NodeOptions & options )
: nav2_util::LifecycleNode( "controller_server", "", options ),
progress_checker_loader_( "nav2_core", "nav2_core::ProgressChecker" ),
default_progress_checker_id_{"progress_checker"},
40 default_progress_checker_type_{"nav2_controller::SimpleProgressChecker"},
41 goal_checker_loader_( "nav2_core", "nav2_core::GoalChecker" ),
default_goal_checker_ids_{"goal_checker"},
default_goal_checker_types_{"nav2_controller::SimpleGoalChecker"},
lp_loader_( "nav2_core", "nav2_core::Controller" ),
default_ids_{"FollowPath"},
default_types_{"dwb_core::DWBLocalPlanner"}
{
RCLCPP_INFO( get_logger( ), "Creating controller server" );
declare_parameter( "controller_frequency", 20.0 );
declare_parameter( "progress_checker_plugin", default_progress_checker_id_ );
declare_parameter( "goal_checker_plugins", default_goal_checker_ids_ );
declare_parameter( "controller_plugins", default_ids_ );
declare_parameter( "min_x_velocity_threshold", rclcpp::ParameterValue( 0.0001 ) );
declare_parameter( "min_y_velocity_threshold", rclcpp::ParameterValue( 0.0001 ) );
declare_parameter( "min_theta_velocity_threshold", rclcpp::ParameterValue( 0.0001 ) );
declare_parameter( "speed_limit_topic", rclcpp::ParameterValue( "speed_limit" ) );
declare_parameter( "failure_tolerance", rclcpp::ParameterValue( 0.0 ) );
// The costmap node is used in the implementation of the controller
costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"local_costmap", std::string{get_namespace( )}, "local_costmap" );
// Launch a thread to run the costmap node
costmap_thread_ = std::make_unique<nav2_util::NodeThread>( costmap_ros_ );
}
ControllerServer::~ControllerServer( )
{
progress_checker_.reset( );
goal_checkers_.clear( );
controllers_.clear( );
costmap_thread_.reset( );
}
nav2_util::CallbackReturn
ControllerServer::on_configure( const rclcpp_lifecycle::State & /*state*/ )
{
auto node = shared_from_this( );
RCLCPP_INFO( get_logger( ), "Configuring controller interface" );
get_parameter( "progress_checker_plugin", progress_checker_id_ );
if ( progress_checker_id_ == default_progress_checker_id_ ) {
nav2_util::declare_parameter_if_not_declared(
node, default_progress_checker_id_ + ".plugin",
rclcpp::ParameterValue( default_progress_checker_type_ ) );
}
RCLCPP_INFO( get_logger( ), "getting goal checker plugins.." );
get_parameter( "goal_checker_plugins", goal_checker_ids_ );
if ( goal_checker_ids_ == default_goal_checker_ids_ ) {
for ( size_t i = 0; i < default_goal_checker_ids_.size( ); ++i ) {
nav2_util::declare_parameter_if_not_declared(
node, default_goal_checker_ids_[i] + ".plugin",
rclcpp::ParameterValue( default_goal_checker_types_[i] ) );
}
}
get_parameter( "controller_plugins", controller_ids_ );
if ( controller_ids_ == default_ids_ ) {
for ( size_t i = 0; i < default_ids_.size( ); ++i ) {
nav2_util::declare_parameter_if_not_declared(
node, default_ids_[i] + ".plugin",
rclcpp::ParameterValue( default_types_[i] ) );
}
}
controller_types_.resize( controller_ids_.size( ) );
goal_checker_types_.resize( goal_checker_ids_.size( ) );
get_parameter( "controller_frequency", controller_frequency_ );
get_parameter( "min_x_velocity_threshold", min_x_velocity_threshold_ );
get_parameter( "min_y_velocity_threshold", min_y_velocity_threshold_ );
get_parameter( "min_theta_velocity_threshold", min_theta_velocity_threshold_ );
RCLCPP_INFO( get_logger( ), "Controller frequency set to %.4fHz", controller_frequency_ );
std::string speed_limit_topic;
get_parameter( "speed_limit_topic", speed_limit_topic );
get_parameter( "failure_tolerance", failure_tolerance_ );
costmap_ros_->configure( );
try {
progress_checker_type_ = nav2_util::get_plugin_type_param( node, progress_checker_id_ );
progress_checker_ = progress_checker_loader_.createUniqueInstance( progress_checker_type_ );
RCLCPP_INFO(
get_logger( ), "Created progress_checker : %s of type %s",
progress_checker_id_.c_str( ), progress_checker_type_.c_str( ) );
progress_checker_->initialize( node, progress_checker_id_ );
} catch ( const pluginlib::PluginlibException & ex ) {
RCLCPP_FATAL(
get_logger( ),
"Failed to create progress_checker. Exception: %s", ex.what( ) );
return nav2_util::CallbackReturn::FAILURE;
}
for ( size_t i = 0; i != goal_checker_ids_.size( ); i++ ) {
try {
goal_checker_types_[i] = nav2_util::get_plugin_type_param( node, goal_checker_ids_[i] );
nav2_core::GoalChecker::Ptr goal_checker =
goal_checker_loader_.createUniqueInstance( goal_checker_types_[i] );
RCLCPP_INFO(
get_logger( ), "Created goal checker : %s of type %s",
goal_checker_ids_[i].c_str( ), goal_checker_types_[i].c_str( ) );
goal_checker->initialize( node, goal_checker_ids_[i], costmap_ros_ );
goal_checkers_.insert( {goal_checker_ids_[i], goal_checker} );
} catch ( const pluginlib::PluginlibException & ex ) {
RCLCPP_FATAL(
get_logger( ),
"Failed to create goal checker. Exception: %s", ex.what( ) );
return nav2_util::CallbackReturn::FAILURE;
}
}
for ( size_t i = 0; i != goal_checker_ids_.size( ); i++ ) {
goal_checker_ids_concat_ += goal_checker_ids_[i] + std::string( " " );
}
RCLCPP_INFO(
get_logger( ),
"Controller Server has %s goal checkers available.", goal_checker_ids_concat_.c_str( ) );
for ( size_t i = 0; i != controller_ids_.size( ); i++ ) {
try {
controller_types_[i] = nav2_util::get_plugin_type_param( node, controller_ids_[i] );
nav2_core::Controller::Ptr controller =
lp_loader_.createUniqueInstance( controller_types_[i] );
RCLCPP_INFO(
get_logger( ), "Created controller : %s of type %s",
controller_ids_[i].c_str( ), controller_types_[i].c_str( ) );
controller->configure(
node, controller_ids_[i],
costmap_ros_->getTfBuffer( ), costmap_ros_ );
controllers_.insert( {controller_ids_[i], controller} );
} catch ( const pluginlib::PluginlibException & ex ) {
RCLCPP_FATAL(
get_logger( ),
"Failed to create controller. Exception: %s", ex.what( ) );
return nav2_util::CallbackReturn::FAILURE;
}
}
for ( size_t i = 0; i != controller_ids_.size( ); i++ ) {
controller_ids_concat_ += controller_ids_[i] + std::string( " " );
}
RCLCPP_INFO(
get_logger( ),
"Controller Server has %s controllers available.", controller_ids_concat_.c_str( ) );
odom_sub_ = std::make_unique<nav_2d_utils::OdomSubscriber>( node );
vel_publisher_ = create_publisher<geometry_msgs::msg::Twist>( "cmd_vel", 1 );
// Create the action server that we implement with our followPath method
action_server_ = std::make_unique<ActionServer>(
shared_from_this( ),
"follow_path",
std::bind( &ControllerServer::computeControl, this ),
nullptr,
std::chrono::milliseconds( 500 ),
true );
// Set subscribtion to the speed limiting topic
speed_limit_sub_ = create_subscription<nav2_msgs::msg::SpeedLimit>(
speed_limit_topic, rclcpp::QoS( 10 ),
std::bind( &ControllerServer::speedLimitCallback, this, std::placeholders::_1 ) );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
ControllerServer::on_activate( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Activating" );
costmap_ros_->activate( );
ControllerMap::iterator it;
for ( it = controllers_.begin( ); it != controllers_.end( ); ++it ) {
it->second->activate( );
}
vel_publisher_->on_activate( );
action_server_->activate( );
auto node = shared_from_this( );
// Add callback for dynamic parameters
dyn_params_handler_ = node->add_on_set_parameters_callback(
std::bind( &ControllerServer::dynamicParametersCallback, this, _1 ) );
// create bond connection
createBond( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
ControllerServer::on_deactivate( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Deactivating" );
action_server_->deactivate( );
ControllerMap::iterator it;
for ( it = controllers_.begin( ); it != controllers_.end( ); ++it ) {
it->second->deactivate( );
}
costmap_ros_->deactivate( );
publishZeroVelocity( );
vel_publisher_->on_deactivate( );
dyn_params_handler_.reset( );
// destroy bond connection
destroyBond( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
ControllerServer::on_cleanup( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Cleaning up" );
// Cleanup the helper classes
ControllerMap::iterator it;
for ( it = controllers_.begin( ); it != controllers_.end( ); ++it ) {
it->second->cleanup( );
}
controllers_.clear( );
goal_checkers_.clear( );
costmap_ros_->cleanup( );
// Release any allocated resources
action_server_.reset( );
odom_sub_.reset( );
vel_publisher_.reset( );
speed_limit_sub_.reset( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
ControllerServer::on_shutdown( const rclcpp_lifecycle::State & )
{
RCLCPP_INFO( get_logger( ), "Shutting down" );
return nav2_util::CallbackReturn::SUCCESS;
}
bool ControllerServer::findControllerId(
const std::string & c_name,
std::string & current_controller )
{
if ( controllers_.find( c_name ) == controllers_.end( ) ) {
if ( controllers_.size( ) == 1 && c_name.empty( ) ) {
RCLCPP_WARN_ONCE(
get_logger( ), "No controller was specified in action call."
" Server will use only plugin loaded %s. "
"This warning will appear once.", controller_ids_concat_.c_str( ) );
current_controller = controllers_.begin( )->first;
} else {
RCLCPP_ERROR(
get_logger( ), "FollowPath called with controller name %s, "
"which does not exist. Available controllers are: %s.",
c_name.c_str( ), controller_ids_concat_.c_str( ) );
return false;
}
} else {
RCLCPP_DEBUG( get_logger( ), "Selected controller: %s.", c_name.c_str( ) );
current_controller = c_name;
}
return true;
}
bool ControllerServer::findGoalCheckerId(
const std::string & c_name,
std::string & current_goal_checker )
{
if ( goal_checkers_.find( c_name ) == goal_checkers_.end( ) ) {
if ( goal_checkers_.size( ) == 1 && c_name.empty( ) ) {
RCLCPP_WARN_ONCE(
get_logger( ), "No goal checker was specified in parameter 'current_goal_checker'."
" Server will use only plugin loaded %s. "
"This warning will appear once.", goal_checker_ids_concat_.c_str( ) );
current_goal_checker = goal_checkers_.begin( )->first;
} else {
RCLCPP_ERROR(
get_logger( ), "FollowPath called with goal_checker name %s in parameter"
" 'current_goal_checker', which does not exist. Available goal checkers are: %s.",
c_name.c_str( ), goal_checker_ids_concat_.c_str( ) );
return false;
}
} else {
RCLCPP_DEBUG( get_logger( ), "Selected goal checker: %s.", c_name.c_str( ) );
current_goal_checker = c_name;
}
return true;
}
void ControllerServer::computeControl( )
{
std::lock_guard<std::mutex> lock( dynamic_params_lock_ );
RCLCPP_INFO( get_logger( ), "Received a goal, begin computing control effort." );
try {
std::string c_name = action_server_->get_current_goal( )->controller_id;
std::string current_controller;
if ( findControllerId( c_name, current_controller ) ) {
current_controller_ = current_controller;
} else {
action_server_->terminate_current( );
return;
}
std::string gc_name = action_server_->get_current_goal( )->goal_checker_id;
std::string current_goal_checker;
if ( findGoalCheckerId( gc_name, current_goal_checker ) ) {
current_goal_checker_ = current_goal_checker;
} else {
action_server_->terminate_current( );
return;
}
setPlannerPath( action_server_->get_current_goal( )->path );
progress_checker_->reset( );
last_valid_cmd_time_ = now( );
rclcpp::WallRate loop_rate( controller_frequency_ );
while ( rclcpp::ok( ) ) {
if ( action_server_ == nullptr || !action_server_->is_server_active( ) ) {
RCLCPP_DEBUG( get_logger( ), "Action server unavailable or inactive. Stopping." );
return;
}
if ( action_server_->is_cancel_requested( ) ) {
RCLCPP_INFO( get_logger( ), "Goal was canceled. Stopping the robot." );
action_server_->terminate_all( );
publishZeroVelocity( );
return;
}
// Don't compute a trajectory until costmap is valid ( after clear costmap )
rclcpp::Rate r( 100 );
while ( !costmap_ros_->isCurrent( ) ) {
r.sleep( );
}
updateGlobalPath( );
computeAndPublishVelocity( );
if ( isGoalReached( ) ) {
RCLCPP_INFO( get_logger( ), "Reached the goal!" );
break;
}
if ( !loop_rate.sleep( ) ) {
RCLCPP_WARN(
get_logger( ), "Control loop missed its desired rate of %.4fHz",
controller_frequency_ );
}
}
} catch ( nav2_core::PlannerException & e ) {
RCLCPP_ERROR( this->get_logger( ), e.what( ) );
publishZeroVelocity( );
action_server_->terminate_current( );
return;
}
RCLCPP_DEBUG( get_logger( ), "Controller succeeded, setting result" );
publishZeroVelocity( );
// TODO( orduno ) #861 Handle a pending preemption and set controller name
action_server_->succeeded_current( );
}
void ControllerServer::setPlannerPath( const nav_msgs::msg::Path & path )
{
RCLCPP_DEBUG(
get_logger( ),
"Providing path to the controller %s", current_controller_.c_str( ) );
if ( path.poses.empty( ) ) {
throw nav2_core::PlannerException( "Invalid path, Path is empty." );
}
controllers_[current_controller_]->setPlan( path );
end_pose_ = path.poses.back( );
end_pose_.header.frame_id = path.header.frame_id;
goal_checkers_[current_goal_checker_]->reset( );
RCLCPP_DEBUG(
get_logger( ), "Path end point is ( %.2f, %.2f )",
end_pose_.pose.position.x, end_pose_.pose.position.y );
current_path_ = path;
}
void ControllerServer::computeAndPublishVelocity( )
{
geometry_msgs::msg::PoseStamped pose;
if ( !getRobotPose( pose ) ) {
throw nav2_core::PlannerException( "Failed to obtain robot pose" );
}
if ( !progress_checker_->check( pose ) ) {
throw nav2_core::PlannerException( "Failed to make progress" );
}
nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist( odom_sub_->getTwist( ) );
geometry_msgs::msg::TwistStamped cmd_vel_2d;
try {
cmd_vel_2d =
controllers_[current_controller_]->computeVelocityCommands(
pose,
nav_2d_utils::twist2Dto3D( twist ),
goal_checkers_[current_goal_checker_].get( ) );
last_valid_cmd_time_ = now( );
} catch ( nav2_core::PlannerException & e ) {
if ( failure_tolerance_ > 0 || failure_tolerance_ == -1.0 ) {
RCLCPP_WARN( this->get_logger( ), e.what( ) );
cmd_vel_2d.twist.angular.x = 0;
cmd_vel_2d.twist.angular.y = 0;
cmd_vel_2d.twist.angular.z = 0;
cmd_vel_2d.twist.linear.x = 0;
cmd_vel_2d.twist.linear.y = 0;
cmd_vel_2d.twist.linear.z = 0;
cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID( );
cmd_vel_2d.header.stamp = now( );
if ( ( now( ) - last_valid_cmd_time_ ).seconds( ) > failure_tolerance_ &&
failure_tolerance_ != -1.0 )
{
throw nav2_core::PlannerException( "Controller patience exceeded" );
}
} else {
throw nav2_core::PlannerException( e.what( ) );
}
}
std::shared_ptr<Action::Feedback> feedback = std::make_shared<Action::Feedback>( );
feedback->speed = std::hypot( cmd_vel_2d.twist.linear.x, cmd_vel_2d.twist.linear.y );
// Find the closest pose to current pose on global path
nav_msgs::msg::Path & current_path = current_path_;
auto find_closest_pose_idx =
[&pose, ¤t_path]( ) {
size_t closest_pose_idx = 0;
double curr_min_dist = std::numeric_limits<double>::max( );
for ( size_t curr_idx = 0; curr_idx < current_path.poses.size( ); ++curr_idx ) {
double curr_dist = nav2_util::geometry_utils::euclidean_distance(
pose, current_path.poses[curr_idx] );
if ( curr_dist < curr_min_dist ) {
curr_min_dist = curr_dist;
closest_pose_idx = curr_idx;
}
}
return closest_pose_idx;
};
feedback->distance_to_goal =
nav2_util::geometry_utils::calculate_path_length( current_path_, find_closest_pose_idx( ) );
action_server_->publish_feedback( feedback );
RCLCPP_DEBUG( get_logger( ), "Publishing velocity at time %.2f", now( ).seconds( ) );
publishVelocity( cmd_vel_2d );
}
void ControllerServer::updateGlobalPath( )
{
if ( action_server_->is_preempt_requested( ) ) {
RCLCPP_INFO( get_logger( ), "Passing new path to controller." );
auto goal = action_server_->accept_pending_goal( );
std::string current_controller;
if ( findControllerId( goal->controller_id, current_controller ) ) {
current_controller_ = current_controller;
} else {
RCLCPP_INFO(
get_logger( ), "Terminating action, invalid controller %s requested.",
goal->controller_id.c_str( ) );
action_server_->terminate_current( );
return;
}
setPlannerPath( goal->path );
}
}
void ControllerServer::publishVelocity( const geometry_msgs::msg::TwistStamped & velocity )
{
auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>( velocity.twist );
if ( vel_publisher_->is_activated( ) && vel_publisher_->get_subscription_count( ) > 0 ) {
vel_publisher_->publish( std::move( cmd_vel ) );
}
}
void ControllerServer::publishZeroVelocity( )
{
geometry_msgs::msg::TwistStamped velocity;
velocity.twist.angular.x = 0;
velocity.twist.angular.y = 0;
velocity.twist.angular.z = 0;
velocity.twist.linear.x = 0;
velocity.twist.linear.y = 0;
velocity.twist.linear.z = 0;
velocity.header.frame_id = costmap_ros_->getBaseFrameID( );
velocity.header.stamp = now( );
publishVelocity( velocity );
}
bool ControllerServer::isGoalReached( )
{
geometry_msgs::msg::PoseStamped pose;
if ( !getRobotPose( pose ) ) {
return false;
}
nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist( odom_sub_->getTwist( ) );
geometry_msgs::msg::Twist velocity = nav_2d_utils::twist2Dto3D( twist );
geometry_msgs::msg::PoseStamped transformed_end_pose;
rclcpp::Duration tolerance( rclcpp::Duration::from_seconds( costmap_ros_->getTransformTolerance( ) ) );
nav_2d_utils::transformPose(
costmap_ros_->getTfBuffer( ), costmap_ros_->getGlobalFrameID( ),
end_pose_, transformed_end_pose, tolerance );
return goal_checkers_[current_goal_checker_]->isGoalReached(
pose.pose, transformed_end_pose.pose,
velocity );
}
bool ControllerServer::getRobotPose( geometry_msgs::msg::PoseStamped & pose )
{
geometry_msgs::msg::PoseStamped current_pose;
if ( !costmap_ros_->getRobotPose( current_pose ) ) {
return false;
}
pose = current_pose;
return true;
}
void ControllerServer::speedLimitCallback( const nav2_msgs::msg::SpeedLimit::SharedPtr msg )
{
ControllerMap::iterator it;
for ( it = controllers_.begin( ); it != controllers_.end( ); ++it ) {
it->second->setSpeedLimit( msg->speed_limit, msg->percentage );
}
}
rcl_interfaces::msg::SetParametersResult
ControllerServer::dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters )
{
rcl_interfaces::msg::SetParametersResult result;
for ( auto parameter : parameters ) {
const auto & type = parameter.get_type( );
const auto & name = parameter.get_name( );
// If we are trying to change the parameter of a plugin we can just skip it at this point
// as they handle parameter changes themselves and don't need to lock the mutex
if ( name.find( '.' ) != std::string::npos ) {
continue;
}
if ( !dynamic_params_lock_.try_lock( ) ) {
RCLCPP_WARN(
get_logger( ),
"Unable to dynamically change Parameters while the controller is currently running" );
result.successful = false;
result.reason =
"Unable to dynamically change Parameters while the controller is currently running";
return result;
}
if ( type == ParameterType::PARAMETER_DOUBLE ) {
if ( name == "controller_frequency" ) {
controller_frequency_ = parameter.as_double( );
} else if ( name == "min_x_velocity_threshold" ) {
min_x_velocity_threshold_ = parameter.as_double( );
} else if ( name == "min_y_velocity_threshold" ) {
min_y_velocity_threshold_ = parameter.as_double( );
} else if ( name == "min_theta_velocity_threshold" ) {
min_theta_velocity_threshold_ = parameter.as_double( );
} else if ( name == "failure_tolerance" ) {
failure_tolerance_ = parameter.as_double( );
}
}
dynamic_params_lock_.unlock( );
}
result.successful = true;
return result;
}
} // namespace nav2_controller
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE( nav2_controller::ControllerServer )
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include "nav2_controller/controller_server.hpp"
#include "rclcpp/rclcpp.hpp"
20 int main( int argc, char ** argv )
{
rclcpp::init( argc, argv );
auto node = std::make_shared<nav2_controller::ControllerServer>( );
rclcpp::spin( node->get_node_base_interface( ) );
rclcpp::shutdown( );
return 0;
}
1 // Copyright ( c ) 2021, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <math.h>
#include <memory>
#include <string>
#include <vector>
#include "gtest/gtest.h"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_controller/controller_server.hpp"
#include "rclcpp/rclcpp.hpp"
26 class ControllerShim : public nav2_controller::ControllerServer
{
public:
29 ControllerShim( )
: nav2_controller::ControllerServer( rclcpp::NodeOptions( ) )
{
}
// Since we cannot call configure/activate due to costmaps
// requiring TF
36 void setDynamicCallback( )
{
auto node = shared_from_this( );
// Add callback for dynamic parameters
dyn_params_handler_ = node->add_on_set_parameters_callback(
std::bind( &ControllerShim::dynamicParamsShim, this, std::placeholders::_1 ) );
}
rcl_interfaces::msg::SetParametersResult
45 dynamicParamsShim( std::vector<rclcpp::Parameter> parameters )
{
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
dynamicParametersCallback( parameters );
return result;
}
};
54 class RclCppFixture
{
public:
57 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
58 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
62 TEST( WPTest, test_dynamic_parameters )
{
auto controller = std::make_shared<ControllerShim>( );
controller->setDynamicCallback( );
auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
controller->get_node_base_interface( ), controller->get_node_topics_interface( ),
controller->get_node_graph_interface( ),
controller->get_node_services_interface( ) );
auto results = rec_param->set_parameters_atomically(
{rclcpp::Parameter( "controller_frequency", 100.0 ),
rclcpp::Parameter( "min_x_velocity_threshold", 100.0 ),
rclcpp::Parameter( "min_y_velocity_threshold", 100.0 ),
rclcpp::Parameter( "min_theta_velocity_threshold", 100.0 ),
rclcpp::Parameter( "failure_tolerance", 5.0 )} );
rclcpp::spin_until_future_complete(
controller->get_node_base_interface( ),
results );
EXPECT_EQ( controller->get_parameter( "controller_frequency" ).as_double( ), 100.0 );
EXPECT_EQ( controller->get_parameter( "min_x_velocity_threshold" ).as_double( ), 100.0 );
EXPECT_EQ( controller->get_parameter( "min_y_velocity_threshold" ).as_double( ), 100.0 );
EXPECT_EQ( controller->get_parameter( "min_theta_velocity_threshold" ).as_double( ), 100.0 );
EXPECT_EQ( controller->get_parameter( "failure_tolerance" ).as_double( ), 5.0 );
}
// Copyright ( c ) 2019 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_CORE__BEHAVIOR_HPP_
#define NAV2_CORE__BEHAVIOR_HPP_
#include <string>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "tf2_ros/buffer.h"
#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
namespace nav2_core
{
/**
* @class Behavior
* @brief Abstract interface for behaviors to adhere to with pluginlib
*/
33 class Behavior
{
public:
using Ptr = std::shared_ptr<Behavior>;
/**
* @brief Virtual destructor
*/
virtual ~Behavior( ) {}
/**
* @param parent pointer to user's node
* @param name The name of this planner
* @param tf A pointer to a TF buffer
* @param costmap_ros A pointer to the costmap
*/
virtual void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
const std::string & name, std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker ) = 0;
/**
* @brief Method to cleanup resources used on shutdown.
*/
57 virtual void cleanup( ) = 0;
/**
* @brief Method to active Behavior and any threads involved in execution.
*/
virtual void activate( ) = 0;
/**
* @brief Method to deactive Behavior and any threads involved in execution.
*/
virtual void deactivate( ) = 0;
};
} // namespace nav2_core
#endif // NAV2_CORE__BEHAVIOR_HPP_
/*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* Copyright ( c ) 2019, Intel Corporation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef NAV2_CORE__CONTROLLER_HPP_
#define NAV2_CORE__CONTROLLER_HPP_
#include <memory>
#include <string>
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "tf2_ros/transform_listener.h"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "pluginlib/class_loader.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
#include "nav_msgs/msg/path.hpp"
#include "nav2_core/goal_checker.hpp"
namespace nav2_core
{
/**
* @class Controller
* @brief controller interface that acts as a virtual base class for all controller plugins
*/
60 class Controller
{
public:
using Ptr = std::shared_ptr<nav2_core::Controller>;
/**
* @brief Virtual destructor
*/
virtual ~Controller( ) {}
/**
* @param parent pointer to user's node
* @param costmap_ros A pointer to the costmap
*/
virtual void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr &,
std::string name, std::shared_ptr<tf2_ros::Buffer>,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> ) = 0;
/**
* @brief Method to cleanup resources.
*/
83 virtual void cleanup( ) = 0;
/**
* @brief Method to active planner and any threads involved in execution.
*/
virtual void activate( ) = 0;
/**
* @brief Method to deactive planner and any threads involved in execution.
*/
virtual void deactivate( ) = 0;
/**
* @brief local setPlan - Sets the global plan
* @param path The global plan
*/
virtual void setPlan( const nav_msgs::msg::Path & path ) = 0;
/**
* @brief Controller computeVelocityCommands - calculates the best command given the current pose and velocity
*
* It is presumed that the global plan is already set.
*
* This is mostly a wrapper for the protected computeVelocityCommands
* function which has additional debugging info.
*
* @param pose Current robot pose
* @param velocity Current robot velocity
* @param goal_checker Pointer to the current goal checker the task is utilizing
* @return The best command for the robot to drive
*/
virtual geometry_msgs::msg::TwistStamped computeVelocityCommands(
const geometry_msgs::msg::PoseStamped & pose,
const geometry_msgs::msg::Twist & velocity,
nav2_core::GoalChecker * goal_checker ) = 0;
/**
* @brief Limits the maximum linear speed of the robot.
* @param speed_limit expressed in absolute value ( in m/s )
* or in percentage from maximum robot speed.
* @param percentage Setting speed limit in percentage if true
* or in absolute values in false case.
*/
virtual void setSpeedLimit( const double & speed_limit, const bool & percentage ) = 0;
};
} // namespace nav2_core
#endif // NAV2_CORE__CONTROLLER_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* Copyright ( c ) 2019, Intel Corporation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef NAV2_CORE__EXCEPTIONS_HPP_
#define NAV2_CORE__EXCEPTIONS_HPP_
#include <stdexcept>
#include <string>
#include <memory>
namespace nav2_core
{
46 class PlannerException : public std::runtime_error
{
public:
49 explicit PlannerException( const std::string description )
: std::runtime_error( description ) {}
using Ptr = std::shared_ptr<PlannerException>;
};
} // namespace nav2_core
#endif // NAV2_CORE__EXCEPTIONS_HPP_
// Copyright ( c ) 2019 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_CORE__GLOBAL_PLANNER_HPP_
#define NAV2_CORE__GLOBAL_PLANNER_HPP_
#include <memory>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "tf2_ros/buffer.h"
#include "nav_msgs/msg/path.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/lifecycle_node.hpp"
namespace nav2_core
{
/**
* @class GlobalPlanner
* @brief Abstract interface for global planners to adhere to with pluginlib
*/
34 class GlobalPlanner
{
public:
using Ptr = std::shared_ptr<GlobalPlanner>;
/**
* @brief Virtual destructor
*/
virtual ~GlobalPlanner( ) {}
/**
* @param parent pointer to user's node
* @param name The name of this planner
* @param tf A pointer to a TF buffer
* @param costmap_ros A pointer to the costmap
*/
virtual void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros ) = 0;
/**
* @brief Method to cleanup resources used on shutdown.
*/
58 virtual void cleanup( ) = 0;
/**
* @brief Method to active planner and any threads involved in execution.
*/
virtual void activate( ) = 0;
/**
* @brief Method to deactive planner and any threads involved in execution.
*/
virtual void deactivate( ) = 0;
/**
* @brief Method create the plan from a starting and ending goal.
* @param start The starting pose of the robot
* @param goal The goal pose of the robot
* @return The sequence of poses to get from start to goal, if any
*/
virtual nav_msgs::msg::Path createPlan(
const geometry_msgs::msg::PoseStamped & start,
const geometry_msgs::msg::PoseStamped & goal ) = 0;
};
} // namespace nav2_core
#endif // NAV2_CORE__GLOBAL_PLANNER_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef NAV2_CORE__GOAL_CHECKER_HPP_
#define NAV2_CORE__GOAL_CHECKER_HPP_
#include <memory>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
namespace nav2_core
{
/**
* @class GoalChecker
* @brief Function-object for checking whether a goal has been reached
*
* This class defines the plugin interface for determining whether you have reached
* the goal state. This primarily consists of checking the relative positions of two poses
* ( which are presumed to be in the same frame ). It can also check the velocity, as some
* applications require that robot be stopped to be considered as having reached the goal.
*/
61 class GoalChecker
{
public:
typedef std::shared_ptr<nav2_core::GoalChecker> Ptr;
66 virtual ~GoalChecker( ) {}
/**
* @brief Initialize any parameters from the NodeHandle
* @param parent Node pointer for grabbing parameters
*/
72 virtual void initialize(
73 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
74 const std::string & plugin_name,
75 const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros ) = 0;
77 virtual void reset( ) = 0;
/**
* @brief Check whether the goal should be considered reached
* @param query_pose The pose to check
* @param goal_pose The pose to check against
* @param velocity The robot's current velocity
* @return True if goal is reached
*/
86 virtual bool isGoalReached(
87 const geometry_msgs::msg::Pose & query_pose, const geometry_msgs::msg::Pose & goal_pose,
88 const geometry_msgs::msg::Twist & velocity ) = 0;
/**
* @brief Get the maximum possible tolerances used for goal checking in the major types.
* Any field without a valid entry is replaced with std::numeric_limits<double>::lowest( )
* to indicate that it is not measured. For tolerance across multiple entries
* ( e.x. XY tolerances ), both fields will contain this value since it is the maximum tolerance
* that each independent field could be assuming the other has no error ( e.x. X and Y ).
* @param pose_tolerance The tolerance used for checking in Pose fields
* @param vel_tolerance The tolerance used for checking velocity fields
* @return True if the tolerances are valid to use
*/
100 virtual bool getTolerances(
101 geometry_msgs::msg::Pose & pose_tolerance,
102 geometry_msgs::msg::Twist & vel_tolerance ) = 0;
};
} // namespace nav2_core
#endif // NAV2_CORE__GOAL_CHECKER_HPP_
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_CORE__PROGRESS_CHECKER_HPP_
#define NAV2_CORE__PROGRESS_CHECKER_HPP_
#include <memory>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose2_d.hpp"
namespace nav2_core
{
/**
* @class nav2_core::ProgressChecker
* @brief This class defines the plugin interface used to check the
* position of the robot to make sure that it is actually progressing
* towards a goal.
*/
34 class ProgressChecker
{
public:
typedef std::shared_ptr<nav2_core::ProgressChecker> Ptr;
39 virtual ~ProgressChecker( ) {}
/**
* @brief Initialize parameters for ProgressChecker
* @param parent Node pointer
*/
45 virtual void initialize(
46 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
47 const std::string & plugin_name ) = 0;
/**
* @brief Checks if the robot has moved compare to previous
* pose
* @param current_pose Current pose of the robot
* @return True if progress is made
*/
54 virtual bool check( geometry_msgs::msg::PoseStamped & current_pose ) = 0;
/**
* @brief Reset class state upon calling
*/
58 virtual void reset( ) = 0;
};
} // namespace nav2_core
#endif // NAV2_CORE__PROGRESS_CHECKER_HPP_
// Copyright ( c ) 2021 RoboTech Vision
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_CORE__SMOOTHER_HPP_
#define NAV2_CORE__SMOOTHER_HPP_
#include <memory>
#include <string>
#include "nav2_costmap_2d/costmap_subscriber.hpp"
#include "nav2_costmap_2d/footprint_subscriber.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "pluginlib/class_loader.hpp"
#include "nav_msgs/msg/path.hpp"
namespace nav2_core
{
/**
* @class Smoother
* @brief smoother interface that acts as a virtual base class for all smoother plugins
*/
38 class Smoother
{
public:
using Ptr = std::shared_ptr<nav2_core::Smoother>;
/**
* @brief Virtual destructor
*/
virtual ~Smoother( ) {}
virtual void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr &,
std::string name, std::shared_ptr<tf2_ros::Buffer>,
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber>,
std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> ) = 0;
/**
* @brief Method to cleanup resources.
*/
57 virtual void cleanup( ) = 0;
/**
* @brief Method to activate smoother and any threads involved in execution.
*/
virtual void activate( ) = 0;
/**
* @brief Method to deactivate smoother and any threads involved in execution.
*/
virtual void deactivate( ) = 0;
/**
* @brief Method to smooth given path
*
* @param path In-out path to be smoothed
* @param max_time Maximum duration smoothing should take
* @return If smoothing was completed ( true ) or interrupted by time limit ( false )
*/
virtual bool smooth(
nav_msgs::msg::Path & path,
const rclcpp::Duration & max_time ) = 0;
};
} // namespace nav2_core
#endif // NAV2_CORE__SMOOTHER_HPP_
1 // Copyright ( c ) 2020 Fetullah Atas
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_CORE__WAYPOINT_TASK_EXECUTOR_HPP_
#define NAV2_CORE__WAYPOINT_TASK_EXECUTOR_HPP_
#pragma once
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
namespace nav2_core
{
/**
* @brief Base class for creating a plugin in order to perform a specific task at waypoint arrivals.
*
*/
32 class WaypointTaskExecutor
{
public:
/**
* @brief Construct a new Simple Task Execution At Waypoint Base object
*
*/
39 WaypointTaskExecutor( ) {}
/**
* @brief Destroy the Simple Task Execution At Waypoint Base object
*
*/
45 virtual ~WaypointTaskExecutor( ) {}
/**
* @brief Override this to setup your pub, sub or any ros services that you will use in the plugin.
*
* @param parent parent node that plugin will be created within( for an example see nav_waypoint_follower )
* @param plugin_name plugin name comes from parameters in yaml file
*/
53 virtual void initialize(
54 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
55 const std::string & plugin_name ) = 0;
/**
* @brief Override this to define the body of your task that you would like to execute once the robot arrived to waypoint
*
* @param curr_pose current pose of the robot
* @param curr_waypoint_index current waypoint, that robot just arrived
* @return true if task execution was successful
* @return false if task execution failed
*/
65 virtual bool processAtWaypoint(
66 const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index ) = 0;
};
} // namespace nav2_core
#endif // NAV2_CORE__WAYPOINT_TASK_EXECUTOR_HPP_
1 /*
* Copyright ( c ) 2012, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES ( INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE )
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* author: Dave Hershberger
*/
#ifndef NAV2_COSTMAP_2D__ARRAY_PARSER_HPP_
#define NAV2_COSTMAP_2D__ARRAY_PARSER_HPP_
#include <vector>
#include <string>
namespace nav2_costmap_2d
{
/** @brief Parse a vector of vectors of floats from a string.
* @param error_return If no error, error_return is set to "". If
* error, error_return will describe the error.
* Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...]
*
* On error, error_return is set and the return value could be
* anything, like part of a successful parse. */
47 std::vector<std::vector<float>> parseVVF( const std::string & input, std::string & error_return );
} // end namespace nav2_costmap_2d
#endif // NAV2_COSTMAP_2D__ARRAY_PARSER_HPP_
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_COSTMAP_2D__CLEAR_COSTMAP_SERVICE_HPP_
#define NAV2_COSTMAP_2D__CLEAR_COSTMAP_SERVICE_HPP_
#include <vector>
#include <string>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "nav2_msgs/srv/clear_costmap_except_region.hpp"
#include "nav2_msgs/srv/clear_costmap_around_robot.hpp"
#include "nav2_msgs/srv/clear_entire_costmap.hpp"
#include "nav2_costmap_2d/costmap_layer.hpp"
#include "nav2_util/lifecycle_node.hpp"
namespace nav2_costmap_2d
{
32 class Costmap2DROS;
/**
* @class ClearCostmapService
* @brief Exposes services to clear costmap objects in inclusive/exclusive regions or completely
*/
38 class ClearCostmapService
{
public:
/**
* @brief A constructor
*/
44 ClearCostmapService( const nav2_util::LifecycleNode::WeakPtr & parent, Costmap2DROS & costmap );
/**
* @brief A constructor
*/
ClearCostmapService( ) = delete;
/**
* @brief Clears the region outside of a user-specified area reverting to the static map
*/
void clearRegion( double reset_distance, bool invert );
/**
* @brief Clears all layers
*/
void clearEntirely( );
private:
// The Logger object for logging
rclcpp::Logger logger_{rclcpp::get_logger( "nav2_costmap_2d" )};
// The costmap to clear
Costmap2DROS & costmap_;
// Clearing parameters
unsigned char reset_value_;
std::vector<std::string> clearable_layers_;
// Server for clearing the costmap
rclcpp::Service<nav2_msgs::srv::ClearCostmapExceptRegion>::SharedPtr clear_except_service_;
/**
* @brief Callback to clear costmap except in a given region
*/
77 void clearExceptRegionCallback(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<nav2_msgs::srv::ClearCostmapExceptRegion::Request> request,
const std::shared_ptr<nav2_msgs::srv::ClearCostmapExceptRegion::Response> response );
rclcpp::Service<nav2_msgs::srv::ClearCostmapAroundRobot>::SharedPtr clear_around_service_;
/**
* @brief Callback to clear costmap in a given region
*/
86 void clearAroundRobotCallback(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundRobot::Request> request,
const std::shared_ptr<nav2_msgs::srv::ClearCostmapAroundRobot::Response> response );
rclcpp::Service<nav2_msgs::srv::ClearEntireCostmap>::SharedPtr clear_entire_service_;
/**
* @brief Callback to clear costmap
*/
95 void clearEntireCallback(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<nav2_msgs::srv::ClearEntireCostmap::Request> request,
const std::shared_ptr<nav2_msgs::srv::ClearEntireCostmap::Response> response );
/**
* @brief Function used to clear a given costmap layer
*/
103 void clearLayerRegion(
std::shared_ptr<CostmapLayer> & costmap, double pose_x, double pose_y, double reset_distance,
bool invert );
/**
* @brief Get the robot's position in the costmap using the master costmap
*/
110 bool getPosition( double & x, double & y ) const;
};
} // namespace nav2_costmap_2d
#endif // NAV2_COSTMAP_2D__CLEAR_COSTMAP_SERVICE_HPP_
1 /*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#ifndef NAV2_COSTMAP_2D__COST_VALUES_HPP_
#define NAV2_COSTMAP_2D__COST_VALUES_HPP_
/** Provides a mapping for often used cost values */
namespace nav2_costmap_2d
{
42 static constexpr unsigned char NO_INFORMATION = 255;
43 static constexpr unsigned char LETHAL_OBSTACLE = 254;
44 static constexpr unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
45 static constexpr unsigned char MAX_NON_OBSTACLE = 252;
46 static constexpr unsigned char FREE_SPACE = 0;
}
#endif // NAV2_COSTMAP_2D__COST_VALUES_HPP_
/*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef NAV2_COSTMAP_2D__COSTMAP_2D_HPP_
#define NAV2_COSTMAP_2D__COSTMAP_2D_HPP_
#include <string.h>
#include <stdio.h>
#include <limits.h>
#include <algorithm>
#include <cmath>
#include <string>
#include <vector>
#include <queue>
#include <mutex>
#include "geometry_msgs/msg/point.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
namespace nav2_costmap_2d
{
// convenient for storing x/y point pairs
struct MapLocation
{
unsigned int x;
unsigned int y;
};
/**
* @class Costmap2D
* @brief A 2D costmap provides a mapping between points in the world and their associated "costs".
*/
67 class Costmap2D
{
69 friend class CostmapTester; // Need this for gtest to work correctly
public:
/**
* @brief Constructor for a costmap
* @param cells_size_x The x size of the map in cells
* @param cells_size_y The y size of the map in cells
* @param resolution The resolution of the map in meters/cell
* @param origin_x The x origin of the map
* @param origin_y The y origin of the map
* @param default_value Default Value
*/
81 Costmap2D(
unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
double origin_x, double origin_y, unsigned char default_value = 0 );
/**
* @brief Copy constructor for a costmap, creates a copy efficiently
* @param map The costmap to copy
*/
89 Costmap2D( const Costmap2D & map );
/**
* @brief Constructor for a costmap from an OccupancyGrid map
* @param map The OccupancyGrid map to create costmap from
*/
95 explicit Costmap2D( const nav_msgs::msg::OccupancyGrid & map );
/**
* @brief Overloaded assignment operator
* @param map The costmap to copy
* @return A reference to the map after the copy has finished
*/
102 Costmap2D & operator=( const Costmap2D & map );
/**
* @brief Turn this costmap into a copy of a window of a costmap passed in
* @param map The costmap to copy
* @param win_origin_x The x origin ( lower left corner ) for the window to copy, in meters
* @param win_origin_y The y origin ( lower left corner ) for the window to copy, in meters
* @param win_size_x The x size of the window, in meters
* @param win_size_y The y size of the window, in meters
*/
112 bool copyCostmapWindow(
const Costmap2D & map, double win_origin_x, double win_origin_y,
double win_size_x,
double win_size_y );
/**
* @brief Copies the ( x0, y0 )..( xn, yn ) window from source costmap into a current costmap
@param source Source costmap where the window will be copied from
@param sx0 Lower x-boundary of the source window to copy, in cells
@param sy0 Lower y-boundary of the source window to copy, in cells
@param sxn Upper x-boundary of the source window to copy, in cells
@param syn Upper y-boundary of the source window to copy, in cells
@param dx0 Lower x-boundary of the destination window to copy, in cells
@param dx0 Lower y-boundary of the destination window to copy, in cells
@returns true if copy was succeeded or false in negative case
*/
128 bool copyWindow(
const Costmap2D & source,
unsigned int sx0, unsigned int sy0, unsigned int sxn, unsigned int syn,
unsigned int dx0, unsigned int dy0 );
/**
* @brief Default constructor
*/
136 Costmap2D( );
/**
* @brief Destructor
*/
141 virtual ~Costmap2D( );
/**
* @brief Get the cost of a cell in the costmap
* @param mx The x coordinate of the cell
* @param my The y coordinate of the cell
* @return The cost of the cell
*/
149 unsigned char getCost( unsigned int mx, unsigned int my ) const;
/**
* @brief Get the cost of a cell in the costmap
* @param mx The x coordinate of the cell
* @param my The y coordinate of the cell
* @return The cost of the cell
*/
157 unsigned char getCost( unsigned int index ) const;
/**
* @brief Set the cost of a cell in the costmap
* @param mx The x coordinate of the cell
* @param my The y coordinate of the cell
* @param cost The cost to set the cell to
*/
165 void setCost( unsigned int mx, unsigned int my, unsigned char cost );
/**
* @brief Convert from map coordinates to world coordinates
* @param mx The x map coordinate
* @param my The y map coordinate
* @param wx Will be set to the associated world x coordinate
* @param wy Will be set to the associated world y coordinate
*/
174 void mapToWorld( unsigned int mx, unsigned int my, double & wx, double & wy ) const;
/**
* @brief Convert from world coordinates to map coordinates
* @param wx The x world coordinate
* @param wy The y world coordinate
* @param mx Will be set to the associated map x coordinate
* @param my Will be set to the associated map y coordinate
* @return True if the conversion was successful ( legal bounds ) false otherwise
*/
184 bool worldToMap( double wx, double wy, unsigned int & mx, unsigned int & my ) const;
/**
* @brief Convert from world coordinates to map coordinates without checking for legal bounds
* @param wx The x world coordinate
* @param wy The y world coordinate
* @param mx Will be set to the associated map x coordinate
* @param my Will be set to the associated map y coordinate
* @note The returned map coordinates <b>are not guaranteed to lie within the map.</b>
*/
194 void worldToMapNoBounds( double wx, double wy, int & mx, int & my ) const;
/**
* @brief Convert from world coordinates to map coordinates, constraining results to legal bounds.
* @param wx The x world coordinate
* @param wy The y world coordinate
* @param mx Will be set to the associated map x coordinate
* @param my Will be set to the associated map y coordinate
* @note The returned map coordinates are guaranteed to lie within the map.
*/
204 void worldToMapEnforceBounds( double wx, double wy, int & mx, int & my ) const;
/**
* @brief Given two map coordinates... compute the associated index
* @param mx The x coordinate
* @param my The y coordinate
* @return The associated index
*/
212 inline unsigned int getIndex( unsigned int mx, unsigned int my ) const
{
return my * size_x_ + mx;
}
/**
* @brief Given an index... compute the associated map coordinates
* @param index The index
* @param mx Will be set to the x coordinate
* @param my Will be set to the y coordinate
*/
223 inline void indexToCells( unsigned int index, unsigned int & mx, unsigned int & my ) const
{
my = index / size_x_;
mx = index - ( my * size_x_ );
}
/**
* @brief Will return a pointer to the underlying unsigned char array used as the costmap
* @return A pointer to the underlying unsigned char array storing cost values
*/
233 unsigned char * getCharMap( ) const;
/**
* @brief Accessor for the x size of the costmap in cells
* @return The x size of the costmap
*/
239 unsigned int getSizeInCellsX( ) const;
/**
* @brief Accessor for the y size of the costmap in cells
* @return The y size of the costmap
*/
245 unsigned int getSizeInCellsY( ) const;
/**
* @brief Accessor for the x size of the costmap in meters
* @return The x size of the costmap ( returns the centerpoint of the last legal cell in the map )
*/
251 double getSizeInMetersX( ) const;
/**
* @brief Accessor for the y size of the costmap in meters
* @return The y size of the costmap ( returns the centerpoint of the last legal cell in the map )
*/
257 double getSizeInMetersY( ) const;
/**
* @brief Accessor for the x origin of the costmap
* @return The x origin of the costmap
*/
263 double getOriginX( ) const;
/**
* @brief Accessor for the y origin of the costmap
* @return The y origin of the costmap
*/
269 double getOriginY( ) const;
/**
* @brief Accessor for the resolution of the costmap
* @return The resolution of the costmap
*/
275 double getResolution( ) const;
/**
* @brief Set the default background value of the costmap
* @param c default value
*/
281 void setDefaultValue( unsigned char c )
{
default_value_ = c;
}
/**
* @brief Get the default background value of the costmap
* @return default value
*/
290 unsigned char getDefaultValue( )
{
return default_value_;
}
/**
* @brief Sets the cost of a convex polygon to a desired value
* @param polygon The polygon to perform the operation on
* @param cost_value The value to set costs to
* @return True if the polygon was filled... false if it could not be filled
*/
301 bool setConvexPolygonCost(
302 const std::vector<geometry_msgs::msg::Point> & polygon,
unsigned char cost_value );
/**
* @brief Get the map cells that make up the outline of a polygon
* @param polygon The polygon in map coordinates to rasterize
* @param polygon_cells Will be set to the cells contained in the outline of the polygon
*/
310 void polygonOutlineCells(
311 const std::vector<MapLocation> & polygon,
312 std::vector<MapLocation> & polygon_cells );
/**
* @brief Get the map cells that fill a convex polygon
* @param polygon The polygon in map coordinates to rasterize
* @param polygon_cells Will be set to the cells that fill the polygon
*/
319 void convexFillCells(
320 const std::vector<MapLocation> & polygon,
321 std::vector<MapLocation> & polygon_cells );
/**
* @brief Move the origin of the costmap to a new location.... keeping data when it can
* @param new_origin_x The x coordinate of the new origin
* @param new_origin_y The y coordinate of the new origin
*/
328 virtual void updateOrigin( double new_origin_x, double new_origin_y );
/**
* @brief Save the costmap out to a pgm file
* @param file_name The name of the file to save
*/
334 bool saveMap( std::string file_name );
/**
* @brief Resize the costmap
*/
339 void resizeMap(
unsigned int size_x, unsigned int size_y, double resolution, double origin_x,
double origin_y );
/**
* @brief Reset the costmap in bounds
*/
346 void resetMap( unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn );
/**
* @brief Reset the costmap in bounds to a value
*/
351 void resetMapToValue(
unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn, unsigned char value );
/**
* @brief Given distance in the world... convert it to cells
* @param world_dist The world distance
* @return The equivalent cell distance
*/
359 unsigned int cellDistance( double world_dist );
// Provide a typedef to ease future code maintenance
typedef std::recursive_mutex mutex_t;
363 mutex_t * getMutex( )
{
return access_;
}
protected:
/**
* @brief Copy a region of a source map into a destination map
* @param source_map The source map
* @param sm_lower_left_x The lower left x point of the source map to start the copy
* @param sm_lower_left_y The lower left y point of the source map to start the copy
* @param sm_size_x The x size of the source map
* @param dest_map The destination map
* @param dm_lower_left_x The lower left x point of the destination map to start the copy
* @param dm_lower_left_y The lower left y point of the destination map to start the copy
* @param dm_size_x The x size of the destination map
* @param region_size_x The x size of the region to copy
* @param region_size_y The y size of the region to copy
*/
template<typename data_type>
void copyMapRegion(
data_type * source_map, unsigned int sm_lower_left_x,
unsigned int sm_lower_left_y,
unsigned int sm_size_x, data_type * dest_map, unsigned int dm_lower_left_x,
unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x,
unsigned int region_size_y )
{
// we'll first need to compute the starting points for each map
data_type * sm_index = source_map + ( sm_lower_left_y * sm_size_x + sm_lower_left_x );
data_type * dm_index = dest_map + ( dm_lower_left_y * dm_size_x + dm_lower_left_x );
// now, we'll copy the source map into the destination map
for ( unsigned int i = 0; i < region_size_y; ++i ) {
memcpy( dm_index, sm_index, region_size_x * sizeof( data_type ) );
sm_index += sm_size_x;
dm_index += dm_size_x;
}
}
/**
* @brief Deletes the costmap, static_map, and markers data structures
*/
virtual void deleteMaps( );
/**
* @brief Resets the costmap and static_map to be unknown space
*/
410 virtual void resetMaps( );
/**
* @brief Initializes the costmap, static_map, and markers data structures
* @param size_x The x size to use for map initialization
* @param size_y The y size to use for map initialization
*/
virtual void initMaps( unsigned int size_x, unsigned int size_y );
/**
* @brief Raytrace a line and apply some action at each step
* @param at The action to take... a functor
* @param x0 The starting x coordinate
* @param y0 The starting y coordinate
* @param x1 The ending x coordinate
* @param y1 The ending y coordinate
* @param max_length The maximum desired length of the segment...
* allows you to not go all the way to the endpoint
* @param min_length The minimum desired length of the segment
*/
template<class ActionType>
inline void raytraceLine(
ActionType at, unsigned int x0, unsigned int y0, unsigned int x1,
unsigned int y1,
unsigned int max_length = UINT_MAX, unsigned int min_length = 0 )
{
int dx_full = x1 - x0;
int dy_full = y1 - y0;
// we need to chose how much to scale our dominant dimension,
// based on the maximum length of the line
double dist = std::hypot( dx_full, dy_full );
if ( dist < min_length ) {
return;
}
unsigned int min_x0, min_y0;
if ( dist > 0.0 ) {
// Adjust starting point and offset to start from min_length distance
min_x0 = ( unsigned int )( x0 + dx_full / dist * min_length );
min_y0 = ( unsigned int )( y0 + dy_full / dist * min_length );
} else {
// dist can be 0 if [x0, y0]==[x1, y1].
// In this case only this cell should be processed.
min_x0 = x0;
min_y0 = y0;
}
unsigned int offset = min_y0 * size_x_ + min_x0;
int dx = x1 - min_x0;
int dy = y1 - min_y0;
unsigned int abs_dx = abs( dx );
unsigned int abs_dy = abs( dy );
int offset_dx = sign( dx );
int offset_dy = sign( dy ) * size_x_;
double scale = ( dist == 0.0 ) ? 1.0 : std::min( 1.0, max_length / dist );
// if x is dominant
if ( abs_dx >= abs_dy ) {
int error_y = abs_dx / 2;
bresenham2D(
at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, ( unsigned int )( scale * abs_dx ) );
return;
}
// otherwise y is dominant
int error_x = abs_dy / 2;
bresenham2D(
at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, ( unsigned int )( scale * abs_dy ) );
}
private:
/**
* @brief A 2D implementation of Bresenham's raytracing algorithm...
* applies an action at each step
*/
template<class ActionType>
inline void bresenham2D(
ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b,
int offset_a,
int offset_b, unsigned int offset,
unsigned int max_length )
{
unsigned int end = std::min( max_length, abs_da );
for ( unsigned int i = 0; i < end; ++i ) {
at( offset );
offset += offset_a;
error_b += abs_db;
if ( ( unsigned int )error_b >= abs_da ) {
offset += offset_b;
error_b -= abs_da;
}
}
at( offset );
}
/**
* @brief get the sign of an int
*/
inline int sign( int x )
{
return x > 0 ? 1.0 : -1.0;
}
mutex_t * access_;
protected:
unsigned int size_x_;
unsigned int size_y_;
double resolution_;
double origin_x_;
double origin_y_;
unsigned char * costmap_;
unsigned char default_value_;
// *INDENT-OFF* Uncrustify doesn't handle indented public/private labels
class MarkCell
{
public:
MarkCell( unsigned char * costmap, unsigned char value )
: costmap_( costmap ), value_( value )
{
}
inline void operator( )( unsigned int offset )
{
costmap_[offset] = value_;
}
private:
unsigned char * costmap_;
unsigned char value_;
};
class PolygonOutlineCells
{
public:
PolygonOutlineCells(
const Costmap2D & costmap, const unsigned char * /*char_map*/,
std::vector<MapLocation> & cells )
: costmap_( costmap ), cells_( cells )
{
}
// just push the relevant cells back onto the list
inline void operator( )( unsigned int offset )
{
MapLocation loc;
costmap_.indexToCells( offset, loc.x, loc.y );
cells_.push_back( loc );
}
private:
const Costmap2D & costmap_;
std::vector<MapLocation> & cells_;
};
// *INDENT-ON*
};
} // namespace nav2_costmap_2d
#endif // NAV2_COSTMAP_2D__COSTMAP_2D_HPP_
/*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* Copyright ( c ) 2019, Samsung Research America, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef NAV2_COSTMAP_2D__COSTMAP_2D_PUBLISHER_HPP_
#define NAV2_COSTMAP_2D__COSTMAP_2D_PUBLISHER_HPP_
#include <algorithm>
#include <string>
#include <memory>
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "map_msgs/msg/occupancy_grid_update.hpp"
#include "nav2_msgs/msg/costmap.hpp"
#include "nav2_msgs/srv/get_costmap.hpp"
#include "tf2/transform_datatypes.h"
#include "nav2_util/lifecycle_node.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
namespace nav2_costmap_2d
{
/**
* @class Costmap2DPublisher
* @brief A tool to periodically publish visualization data from a Costmap2D
*/
63 class Costmap2DPublisher
{
public:
/**
* @brief Constructor for the Costmap2DPublisher
*/
69 Costmap2DPublisher(
70 const nav2_util::LifecycleNode::WeakPtr & parent,
71 Costmap2D * costmap,
72 std::string global_frame,
73 std::string topic_name,
74 bool always_send_full_costmap = false );
/**
* @brief Destructor
*/
79 ~Costmap2DPublisher( );
/**
* @brief Configure node
*/
84 void on_configure( ) {}
/**
* @brief Activate node
*/
89 void on_activate( )
{
costmap_pub_->on_activate( );
costmap_update_pub_->on_activate( );
costmap_raw_pub_->on_activate( );
}
/**
* @brief deactivate node
*/
99 void on_deactivate( )
{
costmap_pub_->on_deactivate( );
costmap_update_pub_->on_deactivate( );
costmap_raw_pub_->on_deactivate( );
}
/**
* @brief Cleanup node
*/
109 void on_cleanup( ) {}
/** @brief Include the given bounds in the changed-rectangle. */
112 void updateBounds( unsigned int x0, unsigned int xn, unsigned int y0, unsigned int yn )
{
x0_ = std::min( x0, x0_ );
xn_ = std::max( xn, xn_ );
y0_ = std::min( y0, y0_ );
yn_ = std::max( yn, yn_ );
}
/**
* @brief Publishes the visualization data over ROS
*/
123 void publishCostmap( );
/**
* @brief Check if the publisher is active
* @return True if the frequency for the publisher is non-zero, false otherwise
*/
129 bool active( )
{
return active_;
}
private:
/** @brief Prepare grid_ message for publication. */
136 void prepareGrid( );
137 void prepareCostmap( );
/** @brief Publish the latest full costmap to the new subscriber. */
// void onNewSubscription( const ros::SingleSubscriberPublisher& pub );
/** @brief GetCostmap callback service */
143 void costmap_service_callback(
144 const std::shared_ptr<rmw_request_id_t> request_header,
145 const std::shared_ptr<nav2_msgs::srv::GetCostmap::Request> request,
146 const std::shared_ptr<nav2_msgs::srv::GetCostmap::Response> response );
148 rclcpp::Clock::SharedPtr clock_;
rclcpp::Logger logger_{rclcpp::get_logger( "nav2_costmap_2d" )};
Costmap2D * costmap_;
std::string global_frame_;
std::string topic_name_;
unsigned int x0_, xn_, y0_, yn_;
double saved_origin_x_;
double saved_origin_y_;
bool active_;
bool always_send_full_costmap_;
// Publisher for translated costmap values as msg::OccupancyGrid used in visualization
rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_pub_;
rclcpp_lifecycle::LifecyclePublisher<map_msgs::msg::OccupancyGridUpdate>::SharedPtr
costmap_update_pub_;
// Publisher for raw costmap values as msg::Costmap from layered costmap
rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::Costmap>::SharedPtr costmap_raw_pub_;
// Service for getting the costmaps
rclcpp::Service<nav2_msgs::srv::GetCostmap>::SharedPtr costmap_service_;
float grid_resolution;
unsigned int grid_width, grid_height;
std::unique_ptr<nav_msgs::msg::OccupancyGrid> grid_;
std::unique_ptr<nav2_msgs::msg::Costmap> costmap_raw_;
// Translate from 0-255 values in costmap to -1 to 100 values in message.
static char * cost_translation_table_;
};
} // namespace nav2_costmap_2d
#endif // NAV2_COSTMAP_2D__COSTMAP_2D_PUBLISHER_HPP_
/*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef NAV2_COSTMAP_2D__COSTMAP_2D_ROS_HPP_
#define NAV2_COSTMAP_2D__COSTMAP_2D_ROS_HPP_
#include <memory>
#include <string>
#include <vector>
#include "geometry_msgs/msg/polygon.h"
#include "geometry_msgs/msg/polygon_stamped.h"
#include "nav2_costmap_2d/costmap_2d_publisher.hpp"
#include "nav2_costmap_2d/footprint.hpp"
#include "nav2_costmap_2d/clear_costmap_service.hpp"
#include "nav2_costmap_2d/layered_costmap.hpp"
#include "nav2_costmap_2d/layer.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "pluginlib/class_loader.hpp"
#include "tf2/convert.h"
#include "tf2/LinearMath/Transform.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tf2/time.h"
#include "tf2/transform_datatypes.h"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpedantic"
#include "tf2/utils.h"
#pragma GCC diagnostic pop
namespace nav2_costmap_2d
{
/** @brief A ROS wrapper for a 2D Costmap. Handles subscribing to
* topics that provide observations about obstacles in either the form
* of PointCloud or LaserScan messages. */
72 class Costmap2DROS : public nav2_util::LifecycleNode
{
public:
/**
* @brief Constructor for the wrapper, the node will
* be placed in a namespace equal to the node's name
* @param name Name of the costmap ROS node
*/
80 explicit Costmap2DROS( const std::string & name );
/**
* @brief Constructor for the wrapper
* @param name Name of the costmap ROS node
* @param parent_namespace Absolute namespace of the node hosting the costmap node
* @param local_namespace Namespace to append to the parent namespace
*/
88 explicit Costmap2DROS(
89 const std::string & name,
90 const std::string & parent_namespace,
91 const std::string & local_namespace );
/**
* @brief A destructor
*/
96 ~Costmap2DROS( );
/**
* @brief Configure node
*/
nav2_util::CallbackReturn on_configure( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Activate node
*/
nav2_util::CallbackReturn on_activate( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Deactivate node
*/
nav2_util::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Cleanup node
*/
nav2_util::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & state ) override;
/**
* @brief shutdown node
*/
nav2_util::CallbackReturn on_shutdown( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Subscribes to sensor topics if necessary and starts costmap
* updates, can be called to restart the costmap after calls to either
* stop( ) or pause( )
*/
void start( );
/**
* @brief Stops costmap updates and unsubscribes from sensor topics
*/
void stop( );
/**
* @brief Stops the costmap from updating, but sensor data still comes in over the wire
*/
void pause( );
/**
* @brief Resumes costmap updates
*/
void resume( );
/**
* @brief Update the map with the layered costmap / plugins
*/
void updateMap( );
/**
* @brief Reset each individual layer
*/
void resetLayers( );
/** @brief Same as getLayeredCostmap( )->isCurrent( ). */
bool isCurrent( )
{
return layered_costmap_->isCurrent( );
}
/**
* @brief Get the pose of the robot in the global frame of the costmap
* @param global_pose Will be set to the pose of the robot in the global frame of the costmap
* @return True if the pose was set successfully, false otherwise
*/
bool getRobotPose( geometry_msgs::msg::PoseStamped & global_pose );
/**
* @brief Transform the input_pose in the global frame of the costmap
* @param input_pose pose to be transformed
* @param transformed_pose pose transformed
* @return True if the pose was transformed successfully, false otherwise
*/
174 bool transformPoseToGlobalFrame(
const geometry_msgs::msg::PoseStamped & input_pose,
geometry_msgs::msg::PoseStamped & transformed_pose );
/** @brief Returns costmap name */
179 std::string getName( ) const
{
return name_;
}
/** @brief Returns the delay in transform ( tf ) data that is tolerable in seconds */
185 double getTransformTolerance( ) const
{
return transform_tolerance_;
}
/**
* @brief Return a pointer to the "master" costmap which receives updates from all the layers.
*
* Same as calling getLayeredCostmap( )->getCostmap( ).
*/
195 Costmap2D * getCostmap( )
{
return layered_costmap_->getCostmap( );
}
/**
* @brief Returns the global frame of the costmap
* @return The global frame of the costmap
*/
204 std::string getGlobalFrameID( )
{
return global_frame_;
}
/**
* @brief Returns the local frame of the costmap
* @return The local frame of the costmap
*/
213 std::string getBaseFrameID( )
{
return robot_base_frame_;
}
/**
* @brief Get the layered costmap object used in the node
*/
221 LayeredCostmap * getLayeredCostmap( )
{
return layered_costmap_.get( );
}
/** @brief Returns the current padded footprint as a geometry_msgs::msg::Polygon. */
227 geometry_msgs::msg::Polygon getRobotFootprintPolygon( )
{
return nav2_costmap_2d::toPolygon( padded_footprint_ );
}
/** @brief Return the current footprint of the robot as a vector of points.
*
* This version of the footprint is padded by the footprint_padding_
* distance, set in the rosparam "footprint_padding".
*
* The footprint initially comes from the rosparam "footprint" but
* can be overwritten by dynamic reconfigure or by messages received
* on the "footprint" topic. */
240 std::vector<geometry_msgs::msg::Point> getRobotFootprint( )
{
return padded_footprint_;
}
/** @brief Return the current unpadded footprint of the robot as a vector of points.
*
* This is the raw version of the footprint without padding.
*
* The footprint initially comes from the rosparam "footprint" but
* can be overwritten by dynamic reconfigure or by messages received
* on the "footprint" topic. */
252 std::vector<geometry_msgs::msg::Point> getUnpaddedRobotFootprint( )
{
return unpadded_footprint_;
}
/**
* @brief Build the oriented footprint of the robot at the robot's current pose
* @param oriented_footprint Will be filled with the points in the oriented footprint of the robot
*/
261 void getOrientedFootprint( std::vector<geometry_msgs::msg::Point> & oriented_footprint );
/** @brief Set the footprint of the robot to be the given set of
* points, padded by footprint_padding.
*
* Should be a convex polygon, though this is not enforced.
*
* First expands the given polygon by footprint_padding_ and then
* sets padded_footprint_ and calls
* layered_costmap_->setFootprint( ). Also saves the unpadded
* footprint, which is available from
* getUnpaddedRobotFootprint( ). */
273 void setRobotFootprint( const std::vector<geometry_msgs::msg::Point> & points );
/** @brief Set the footprint of the robot to be the given polygon,
* padded by footprint_padding.
*
* Should be a convex polygon, though this is not enforced.
*
* First expands the given polygon by footprint_padding_ and then
* sets padded_footprint_ and calls
* layered_costmap_->setFootprint( ). Also saves the unpadded
* footprint, which is available from
* getUnpaddedRobotFootprint( ). */
285 void setRobotFootprintPolygon( const geometry_msgs::msg::Polygon::SharedPtr footprint );
287 std::shared_ptr<tf2_ros::Buffer> getTfBuffer( ) {return tf_buffer_;}
/**
* @brief Get the costmap's use_radius_ parameter, corresponding to
* whether the footprint for the robot is a circle with radius robot_radius_
* or an arbitrarily defined footprint in footprint_.
* @return use_radius_
*/
295 bool getUseRadius( ) {return use_radius_;}
/**
* @brief Get the costmap's robot_radius_ parameter, corresponding to
* raidus of the robot footprint when it is defined as as circle
* ( i.e. when use_radius_ == true ).
* @return robot_radius_
*/
303 double getRobotRadius( ) {return robot_radius_;}
protected:
// Publishers and subscribers
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PolygonStamped>::SharedPtr
footprint_pub_;
309 std::unique_ptr<Costmap2DPublisher> costmap_publisher_{nullptr};
rclcpp::Subscription<geometry_msgs::msg::Polygon>::SharedPtr footprint_sub_;
rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr parameter_sub_;
// Dedicated callback group and executor for tf timer_interface and message fillter
rclcpp::CallbackGroup::SharedPtr callback_group_;
rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
std::unique_ptr<nav2_util::NodeThread> executor_thread_;
// Transform listener
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
std::unique_ptr<LayeredCostmap> layered_costmap_{nullptr};
std::string name_;
std::string parent_namespace_;
/**
* @brief Function on timer for costmap update
*/
void mapUpdateLoop( double frequency );
bool map_update_thread_shutdown_{false};
bool stop_updates_{false};
bool initialized_{false};
bool stopped_{true};
std::unique_ptr<std::thread> map_update_thread_; ///< @brief A thread for updating the map
rclcpp::Time last_publish_{0, 0, RCL_ROS_TIME};
rclcpp::Duration publish_cycle_{1, 0};
pluginlib::ClassLoader<Layer> plugin_loader_{"nav2_costmap_2d", "nav2_costmap_2d::Layer"};
/**
* @brief Get parameters for node
*/
void getParameters( );
bool always_send_full_costmap_{false};
std::string footprint_;
float footprint_padding_{0};
std::string global_frame_; ///< The global frame for the costmap
int map_height_meters_{0};
double map_publish_frequency_{0};
double map_update_frequency_{0};
int map_width_meters_{0};
double origin_x_{0};
double origin_y_{0};
std::vector<std::string> default_plugins_;
std::vector<std::string> default_types_;
std::vector<std::string> plugin_names_;
std::vector<std::string> plugin_types_;
std::vector<std::string> filter_names_;
std::vector<std::string> filter_types_;
double resolution_{0};
std::string robot_base_frame_; ///< The frame_id of the robot base
double robot_radius_;
bool rolling_window_{false}; ///< Whether to use a rolling window version of the costmap
bool track_unknown_space_{false};
double transform_tolerance_{0}; ///< The timeout before transform errors
// Derived parameters
bool use_radius_{false};
std::vector<geometry_msgs::msg::Point> unpadded_footprint_;
std::vector<geometry_msgs::msg::Point> padded_footprint_;
std::unique_ptr<ClearCostmapService> clear_costmap_service_;
// Dynamic parameters handler
OnSetParametersCallbackHandle::SharedPtr dyn_params_handler;
/**
* @brief Callback executed when a paramter change is detected
* @param parameters list of changed parameters
*/
rcl_interfaces::msg::SetParametersResult
dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters );
};
} // namespace nav2_costmap_2d
#endif // NAV2_COSTMAP_2D__COSTMAP_2D_ROS_HPP_
/*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2020 Samsung Research Russia
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the <ORGANIZATION> nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Alexey Merzlyakov
*********************************************************************/
#ifndef NAV2_COSTMAP_2D__COSTMAP_FILTERS__COSTMAP_FILTER_HPP_
#define NAV2_COSTMAP_2D__COSTMAP_FILTERS__COSTMAP_FILTER_HPP_
#include <string>
#include <mutex>
#include "geometry_msgs/msg/pose2_d.hpp"
#include "nav2_costmap_2d/layer.hpp"
namespace nav2_costmap_2d
{
/**
* @brief: CostmapFilter basic class. It is inherited from Layer in order to avoid
* hidden problems when the shared handling of costmap_ resource ( PR #1936 )
*/
54 class CostmapFilter : public Layer
{
public:
/**
* @brief A constructor
*/
60 CostmapFilter( );
/**
* @brief A destructor
*/
64 ~CostmapFilter( );
/**
* @brief: Provide a typedef to ease future code maintenance
*/
typedef std::recursive_mutex mutex_t;
/**
* @brief: returns pointer to a mutex
*/
73 mutex_t * getMutex( )
{
return access_;
}
/**
* @brief Initialization process of layer on startup
*/
void onInitialize( ) final;
/**
* @brief Update the bounds of the master costmap by this layer's update dimensions
* @param robot_x X pose of robot
* @param robot_y Y pose of robot
* @param robot_yaw Robot orientation
* @param min_x X min map coord of the window to update
* @param min_y Y min map coord of the window to update
* @param max_x X max map coord of the window to update
* @param max_y Y max map coord of the window to update
*/
void updateBounds(
double robot_x, double robot_y, double robot_yaw,
double * min_x, double * min_y, double * max_x, double * max_y ) final;
/**
* @brief Update the costs in the master costmap in the window
* @param master_grid The master costmap grid to update
* @param min_x X min map coord of the window to update
* @param min_y Y min map coord of the window to update
* @param max_x X max map coord of the window to update
* @param max_y Y max map coord of the window to update
*/
void updateCosts(
nav2_costmap_2d::Costmap2D & master_grid,
int min_i, int min_j, int max_i, int max_j ) final;
/**
* @brief Activate the layer
*/
void activate( ) final;
/**
* @brief Deactivate the layer
*/
void deactivate( ) final;
/**
* @brief Reset the layer
*/
void reset( ) final;
/**
* @brief If clearing operations should be processed on this layer or not
*/
bool isClearable( ) {return false;}
/** CostmapFilter API **/
/**
* @brief: Initializes costmap filter. Creates subscriptions to filter-related topics
* @param: Name of costmap filter info topic
*/
virtual void initializeFilter(
const std::string & filter_info_topic ) = 0;
/**
* @brief: An algorithm for how to use that map's information. Fills the Costmap2D with
* calculated data and makes an action based on processed data
* @param: Reference to a master costmap2d
* @param: Low window map boundary OX
* @param: Low window map boundary OY
* @param: High window map boundary OX
* @param: High window map boundary OY
* @param: Robot 2D-pose
*/
145 virtual void process(
nav2_costmap_2d::Costmap2D & master_grid,
int min_i, int min_j, int max_i, int max_j,
const geometry_msgs::msg::Pose2D & pose ) = 0;
/**
* @brief: Resets costmap filter. Stops all subscriptions
*/
virtual void resetFilter( ) = 0;
protected:
/**
* @brief: Name of costmap filter info topic
*/
std::string filter_info_topic_;
/**
* @brief: Name of filter mask topic
*/
std::string mask_topic_;
/**
* @brief: mask_frame_->global_frame_ transform tolerance
*/
tf2::Duration transform_tolerance_;
private:
/**
* @brief: Latest robot position
*/
geometry_msgs::msg::Pose2D latest_pose_;
/**
* @brief: Mutex for locking filter's resources
*/
mutex_t * access_;
};
} // namespace nav2_costmap_2d
#endif // NAV2_COSTMAP_2D__COSTMAP_FILTERS__COSTMAP_FILTER_HPP_
1 /*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2020 Samsung Research Russia
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the <ORGANIZATION> nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Alexey Merzlyakov
*********************************************************************/
#ifndef NAV2_COSTMAP_2D__COSTMAP_FILTERS__FILTER_VALUES_HPP_
#define NAV2_COSTMAP_2D__COSTMAP_FILTERS__FILTER_VALUES_HPP_
/** Provides constants used in costmap filters */
namespace nav2_costmap_2d
{
/** Types of costmap filter */
static constexpr uint8_t KEEPOUT_FILTER = 0;
static constexpr uint8_t SPEED_FILTER_PERCENT = 1;
static constexpr uint8_t SPEED_FILTER_ABSOLUTE = 2;
/** Default values for base and multiplier */
52 static constexpr double BASE_DEFAULT = 0.0;
53 static constexpr double MULTIPLIER_DEFAULT = 1.0;
/** Speed filter constants */
static constexpr int8_t SPEED_MASK_UNKNOWN = -1;
static constexpr int8_t SPEED_MASK_NO_LIMIT = 0;
58 static constexpr double NO_SPEED_LIMIT = 0.0;
} // namespace nav2_costmap_2d
#endif // NAV2_COSTMAP_2D__COSTMAP_FILTERS__FILTER_VALUES_HPP_
1 /*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2020 Samsung Research Russia
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the <ORGANIZATION> nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Alexey Merzlyakov
*********************************************************************/
#ifndef NAV2_COSTMAP_2D__COSTMAP_FILTERS__KEEPOUT_FILTER_HPP_
#define NAV2_COSTMAP_2D__COSTMAP_FILTERS__KEEPOUT_FILTER_HPP_
#include <string>
#include <memory>
#include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp"
#include "rclcpp/rclcpp.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "nav2_msgs/msg/costmap_filter_info.hpp"
namespace nav2_costmap_2d
{
/**
* @class KeepoutFilter
* @brief Reads in a keepout mask and marks keepout regions in the map
* to prevent planning or control in restricted areas
*/
58 class KeepoutFilter : public CostmapFilter
{
public:
/**
* @brief A constructor
*/
64 KeepoutFilter( );
/**
* @brief Initialize the filter and subscribe to the info topic
*/
69 void initializeFilter(
70 const std::string & filter_info_topic );
/**
* @brief Process the keepout layer at the current pose / bounds / grid
*/
75 void process(
76 nav2_costmap_2d::Costmap2D & master_grid,
int min_i, int min_j, int max_i, int max_j,
78 const geometry_msgs::msg::Pose2D & pose );
/**
* @brief Reset the costmap filter / topic / info
*/
83 void resetFilter( );
/**
* @brief If this filter is active
*/
88 bool isActive( );
private:
/**
* @brief Callback for the filter information
*/
94 void filterInfoCallback( const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg );
/**
* @brief Callback for the filter mask
*/
98 void maskCallback( const nav_msgs::msg::OccupancyGrid::SharedPtr msg );
100 rclcpp::Subscription<nav2_msgs::msg::CostmapFilterInfo>::SharedPtr filter_info_sub_;
101 rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr mask_sub_;
103 std::unique_ptr<Costmap2D> mask_costmap_;
105 std::string mask_frame_; // Frame where mask located in
106 std::string global_frame_; // Frame of currnet layer ( master_grid )
};
} // namespace nav2_costmap_2d
#endif // NAV2_COSTMAP_2D__COSTMAP_FILTERS__KEEPOUT_FILTER_HPP_
1 /*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2020 Samsung Research Russia
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the <ORGANIZATION> nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Alexey Merzlyakov
*********************************************************************/
#ifndef NAV2_COSTMAP_2D__COSTMAP_FILTERS__SPEED_FILTER_HPP_
#define NAV2_COSTMAP_2D__COSTMAP_FILTERS__SPEED_FILTER_HPP_
#include <memory>
#include <string>
#include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "nav2_msgs/msg/costmap_filter_info.hpp"
#include "nav2_msgs/msg/speed_limit.hpp"
namespace nav2_costmap_2d
{
/**
* @class SpeedFilter
* @brief Reads in a speed restriction mask and enables a robot to
* dynamically adjust speed based on pose in map to slow in dangerous
* areas. Done via absolute speed setting or percentage of maximum speed
*/
58 class SpeedFilter : public CostmapFilter
{
public:
/**
* @brief A constructor
*/
64 SpeedFilter( );
/**
* @brief Initialize the filter and subscribe to the info topic
*/
69 void initializeFilter(
70 const std::string & filter_info_topic );
/**
* @brief Process the keepout layer at the current pose / bounds / grid
*/
75 void process(
76 nav2_costmap_2d::Costmap2D & master_grid,
int min_i, int min_j, int max_i, int max_j,
78 const geometry_msgs::msg::Pose2D & pose );
/**
* @brief Reset the costmap filter / topic / info
*/
83 void resetFilter( );
/**
* @brief If this filter is active
*/
88 bool isActive( );
protected:
/**
* @brief: Transforms robot pose from current layer frame to mask frame
* @param: pose Robot pose in costmap frame
* @param: mask_pose Robot pose in mask frame
* @return: True if the transformation was successful, false otherwise
*/
97 bool transformPose(
98 const geometry_msgs::msg::Pose2D & pose,
99 geometry_msgs::msg::Pose2D & mask_pose ) const;
/**
* @brief: Convert from world coordinates to mask coordinates.
Similar to Costmap2D::worldToMap( ) method but works directly with OccupancyGrid-s.
* @param wx The x world coordinate
* @param wy The y world coordinate
* @param mx Will be set to the associated mask x coordinate
* @param my Will be set to the associated mask y coordinate
* @return True if the conversion was successful ( legal bounds ) false otherwise
*/
110 bool worldToMask( double wx, double wy, unsigned int & mx, unsigned int & my ) const;
/**
* @brief Get the data of a cell in the filter mask
* @param mx The x coordinate of the cell
* @param my The y coordinate of the cell
* @return The data of the selected cell
*/
118 inline int8_t getMaskData(
const unsigned int mx, const unsigned int my ) const;
private:
/**
* @brief Callback for the filter information
*/
125 void filterInfoCallback( const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg );
/**
* @brief Callback for the filter mask
*/
129 void maskCallback( const nav_msgs::msg::OccupancyGrid::SharedPtr msg );
131 rclcpp::Subscription<nav2_msgs::msg::CostmapFilterInfo>::SharedPtr filter_info_sub_;
132 rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr mask_sub_;
134 rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::SpeedLimit>::SharedPtr speed_limit_pub_;
136 nav_msgs::msg::OccupancyGrid::SharedPtr filter_mask_;
138 std::string mask_frame_; // Frame where mask located in
139 std::string global_frame_; // Frame of currnet layer ( master_grid )
double base_, multiplier_;
142 bool percentage_;
double speed_limit_, speed_limit_prev_;
};
} // namespace nav2_costmap_2d
#endif // NAV2_COSTMAP_2D__COSTMAP_FILTERS__SPEED_FILTER_HPP_
/*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef NAV2_COSTMAP_2D__COSTMAP_LAYER_HPP_
#define NAV2_COSTMAP_2D__COSTMAP_LAYER_HPP_
#include <rclcpp/rclcpp.hpp>
#include <nav2_costmap_2d/layer.hpp>
#include <nav2_costmap_2d/layered_costmap.hpp>
namespace nav2_costmap_2d
{
/**
* @class CostmapLayer
* @brief A costmap layer base class for costmap plugin layers.
* Rather than just a layer, this object also contains an internal
* costmap object to populate and maintain state.
*/
54 class CostmapLayer : public Layer, public Costmap2D
{
public:
/**
* @brief CostmapLayer constructor
*/
CostmapLayer( )
: has_extra_bounds_( false ),
extra_min_x_( 1e6 ), extra_max_x_( -1e6 ),
extra_min_y_( 1e6 ), extra_max_y_( -1e6 ) {}
/**
* @brief If layer is discrete
*/
bool isDiscretized( )
{
return true;
}
/**
* @brief Match the size of the master costmap
*/
76 virtual void matchSize( );
/**
* @brief Clear an are in the costmap with the given dimension
* if invert, then clear everything except these dimensions
*/
virtual void clearArea( int start_x, int start_y, int end_x, int end_y, bool invert );
/**
* If an external source changes values in the costmap,
* it should call this method with the area that it changed
* to ensure that the costmap includes this region as well.
* @param mx0 Minimum x value of the bounding box
* @param my0 Minimum y value of the bounding box
* @param mx1 Maximum x value of the bounding box
* @param my1 Maximum y value of the bounding box
*/
void addExtraBounds( double mx0, double my0, double mx1, double my1 );
protected:
/*
* Updates the master_grid within the specified
* bounding box using this layer's values.
*
* TrueOverwrite means every value from this layer
* is written into the master grid.
*/
void updateWithTrueOverwrite(
nav2_costmap_2d::Costmap2D & master_grid,
int min_i, int min_j, int max_i, int max_j );
/*
* Updates the master_grid within the specified
* bounding box using this layer's values.
*
* Overwrite means every valid value from this layer
* is written into the master grid ( does not copy NO_INFORMATION )
*/
void updateWithOverwrite(
nav2_costmap_2d::Costmap2D & master_grid,
int min_i, int min_j, int max_i, int max_j );
/*
* Updates the master_grid within the specified
* bounding box using this layer's values.
*
* Sets the new value to the maximum of the master_grid's value
* and this layer's value. If the master value is NO_INFORMATION,
* it is overwritten. If the layer's value is NO_INFORMATION,
* the master value does not change.
*/
void updateWithMax(
nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i,
int max_j );
/*
* Updates the master_grid within the specified
* bounding box using this layer's values.
*
* Sets the new value to the sum of the master grid's value
* and this layer's value. If the master value is NO_INFORMATION,
* it is overwritten with the layer's value. If the layer's value
* is NO_INFORMATION, then the master value does not change.
*
* If the sum value is larger than INSCRIBED_INFLATED_OBSTACLE,
* the master value is set to ( INSCRIBED_INFLATED_OBSTACLE - 1 ).
*/
void updateWithAddition(
nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i,
int max_j );
/**
* Updates the bounding box specified in the parameters to include
* the location ( x, y )
*
* @param x x-coordinate to include
* @param y y-coordinate to include
* @param min_x bounding box
* @param min_y bounding box
* @param max_x bounding box
* @param max_y bounding box
*/
void touch( double x, double y, double * min_x, double * min_y, double * max_x, double * max_y );
/*
* Updates the bounding box specified in the parameters
* to include the bounding box from the addExtraBounds
* call. If addExtraBounds was not called, the method will do nothing.
*
* Should be called at the beginning of the updateBounds method
*
* @param min_x bounding box ( input and output )
* @param min_y bounding box ( input and output )
* @param max_x bounding box ( input and output )
* @param max_y bounding box ( input and output )
*/
void useExtraBounds( double * min_x, double * min_y, double * max_x, double * max_y );
bool has_extra_bounds_;
private:
double extra_min_x_, extra_max_x_, extra_min_y_, extra_max_y_;
};
} // namespace nav2_costmap_2d
#endif // NAV2_COSTMAP_2D__COSTMAP_LAYER_HPP_
1 /*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef NAV2_COSTMAP_2D__COSTMAP_MATH_HPP_
#define NAV2_COSTMAP_2D__COSTMAP_MATH_HPP_
#include <math.h>
#include <algorithm>
#include <vector>
#include "geometry_msgs/msg/point.hpp"
/** @brief Return -1 if x < 0, +1 otherwise. */
48 inline double sign( double x )
{
return x < 0.0 ? -1.0 : 1.0;
}
/** @brief Same as sign( x ) but returns 0 if x is 0. */
54 inline double sign0( double x )
{
return x < 0.0 ? -1.0 : ( x > 0.0 ? 1.0 : 0.0 );
}
/** @brief Gets L2 norm distance */
60 inline double distance( double x0, double y0, double x1, double y1 )
{
return hypot( x1 - x0, y1 - y0 );
}
/** @brief Gets point distance to a line */
66 double distanceToLine( double pX, double pY, double x0, double y0, double x1, double y1 );
#endif // NAV2_COSTMAP_2D__COSTMAP_MATH_HPP_
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_COSTMAP_2D__COSTMAP_SUBSCRIBER_HPP_
#define NAV2_COSTMAP_2D__COSTMAP_SUBSCRIBER_HPP_
#include <string>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_msgs/msg/costmap.hpp"
#include "nav2_util/lifecycle_node.hpp"
namespace nav2_costmap_2d
{
/**
* @class CostmapSubscriber
* @brief Subscribes to the costmap via a ros topic
*/
32 class CostmapSubscriber
{
public:
/**
* @brief A constructor
*/
38 CostmapSubscriber(
39 const nav2_util::LifecycleNode::WeakPtr & parent,
40 const std::string & topic_name );
/**
* @brief A constructor
*/
45 CostmapSubscriber(
46 const rclcpp::Node::WeakPtr & parent,
47 const std::string & topic_name );
/**
* @brief A destructor
*/
52 ~CostmapSubscriber( ) {}
/**
* @brief A Get the costmap from topic
*/
57 std::shared_ptr<Costmap2D> getCostmap( );
/**
* @brief Convert an occ grid message into a costmap object
*/
62 void toCostmap2D( );
/**
* @brief Callback for the costmap topic
*/
66 void costmapCallback( const nav2_msgs::msg::Costmap::SharedPtr msg );
protected:
69 std::shared_ptr<Costmap2D> costmap_;
70 nav2_msgs::msg::Costmap::SharedPtr costmap_msg_;
71 std::string topic_name_;
bool costmap_received_{false};
rclcpp::Subscription<nav2_msgs::msg::Costmap>::SharedPtr costmap_sub_;
};
} // namespace nav2_costmap_2d
#endif // NAV2_COSTMAP_2D__COSTMAP_SUBSCRIBER_HPP_
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Modified by: Shivang Patel ( shivaan14@gmail.com )
#ifndef NAV2_COSTMAP_2D__COSTMAP_TOPIC_COLLISION_CHECKER_HPP_
#define NAV2_COSTMAP_2D__COSTMAP_TOPIC_COLLISION_CHECKER_HPP_
#include <string>
#include <vector>
#include <memory>
#include <algorithm>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose2_d.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/footprint_collision_checker.hpp"
#include "nav2_costmap_2d/costmap_subscriber.hpp"
#include "nav2_costmap_2d/footprint_subscriber.hpp"
namespace nav2_costmap_2d
{
/**
* @class CostmapTopicCollisionChecker
* @brief Using a costmap via a ros topic, this object is used to
* find if robot poses are in collision with the costmap environment
*/
40 class CostmapTopicCollisionChecker
{
public:
/**
* @brief A constructor
*/
46 CostmapTopicCollisionChecker(
47 CostmapSubscriber & costmap_sub,
48 FootprintSubscriber & footprint_sub,
49 std::string name = "collision_checker" );
/**
* @brief A destructor
*/
~CostmapTopicCollisionChecker( ) = default;
/**
* @brief Returns the obstacle footprint score for a particular pose
*
* @param pose Pose to get score at
* @param fetch_costmap_and_footprint Defaults to true. When checking with multiple poses at once,
* data should be fetched in the first check but fetching can be skipped in consequent checks for speedup
*/
double scorePose(
const geometry_msgs::msg::Pose2D & pose,
bool fetch_costmap_and_footprint = true );
/**
* @brief Returns if a pose is collision free
*
* @param pose Pose to check collision at
* @param fetch_costmap_and_footprint Defaults to true. When checking with multiple poses at once,
* data should be fetched in the first check but fetching can be skipped in consequent checks for speedup
*/
bool isCollisionFree(
const geometry_msgs::msg::Pose2D & pose,
bool fetch_costmap_and_footprint = true );
protected:
/**
* @brief Get a footprint at a set pose
*
* @param pose Pose to get footprint at
* @param fetch_latest_footprint Defaults to true. When checking with multiple poses at once,
* footprint should be fetched in the first check but fetching can be skipped in consequent checks for speedup
*/
Footprint getFootprint(
const geometry_msgs::msg::Pose2D & pose,
bool fetch_latest_footprint = true );
// Name used for logging
std::string name_;
CostmapSubscriber & costmap_sub_;
FootprintSubscriber & footprint_sub_;
FootprintCollisionChecker<std::shared_ptr<Costmap2D>> collision_checker_;
rclcpp::Clock::SharedPtr clock_;
Footprint footprint_;
};
} // namespace nav2_costmap_2d
#endif // NAV2_COSTMAP_2D__COSTMAP_TOPIC_COLLISION_CHECKER_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef NAV2_COSTMAP_2D__EXCEPTIONS_HPP_
#define NAV2_COSTMAP_2D__EXCEPTIONS_HPP_
#include <stdexcept>
#include <string>
#include <memory>
namespace nav2_costmap_2d
{
/**
* @class CollisionCheckerException
* @brief Exceptions thrown if collision checker determines a pose is in
* collision with the environment costmap
*/
48 class CollisionCheckerException : public std::runtime_error
{
public:
51 explicit CollisionCheckerException( const std::string description )
: std::runtime_error( description ) {}
};
/**
* @class IllegalPoseException
* @brief Thrown when CollisionChecker encounters a fatal error
*/
59 class IllegalPoseException : public CollisionCheckerException
{
public:
62 IllegalPoseException( const std::string name, const std::string description )
: CollisionCheckerException( description ), name_( name ) {}
64 std::string getCriticName( ) const {return name_;}
protected:
67 std::string name_;
};
} // namespace nav2_costmap_2d
#endif // NAV2_COSTMAP_2D__EXCEPTIONS_HPP_
1 /*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef NAV2_COSTMAP_2D__FOOTPRINT_HPP_
#define NAV2_COSTMAP_2D__FOOTPRINT_HPP_
#include <string>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/polygon.hpp"
#include "geometry_msgs/msg/polygon_stamped.hpp"
#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/point32.hpp"
#include "nav2_util/lifecycle_node.hpp"
namespace nav2_costmap_2d
{
/**
* @brief Calculate the extreme distances for the footprint
*
* @param footprint The footprint to examine
* @param min_dist Output parameter of the minimum distance
* @param max_dist Output parameter of the maximum distance
*/
61 void calculateMinAndMaxDistances(
const std::vector<geometry_msgs::msg::Point> & footprint,
double & min_dist, double & max_dist );
/**
* @brief Convert Point32 to Point
*/
68 geometry_msgs::msg::Point toPoint( geometry_msgs::msg::Point32 pt );
/**
* @brief Convert Point to Point32
*/
73 geometry_msgs::msg::Point32 toPoint32( geometry_msgs::msg::Point pt );
/**
* @brief Convert vector of Points to Polygon msg
*/
78 geometry_msgs::msg::Polygon toPolygon( std::vector<geometry_msgs::msg::Point> pts );
/**
* @brief Convert Polygon msg to vector of Points.
*/
83 std::vector<geometry_msgs::msg::Point> toPointVector(
geometry_msgs::msg::Polygon::SharedPtr polygon );
/**
* @brief Given a pose and base footprint, build the oriented footprint of the robot ( list of Points )
* @param x The x position of the robot
* @param y The y position of the robot
* @param theta The orientation of the robot
* @param footprint_spec Basic shape of the footprint
* @param oriented_footprint Will be filled with the points in the oriented footprint of the robot
*/
94 void transformFootprint(
double x, double y, double theta,
const std::vector<geometry_msgs::msg::Point> & footprint_spec,
std::vector<geometry_msgs::msg::Point> & oriented_footprint );
/**
* @brief Given a pose and base footprint, build the oriented footprint of the robot ( PolygonStamped )
* @param x The x position of the robot
* @param y The y position of the robot
* @param theta The orientation of the robot
* @param footprint_spec Basic shape of the footprint
* @param oriented_footprint Will be filled with the points in the oriented footprint of the robot
*/
107 void transformFootprint(
double x, double y, double theta,
const std::vector<geometry_msgs::msg::Point> & footprint_spec,
geometry_msgs::msg::PolygonStamped & oriented_footprint );
/**
* @brief Adds the specified amount of padding to the footprint ( in place )
*/
115 void padFootprint( std::vector<geometry_msgs::msg::Point> & footprint, double padding );
/**
* @brief Create a circular footprint from a given radius
*/
120 std::vector<geometry_msgs::msg::Point> makeFootprintFromRadius( double radius );
/**
* @brief Make the footprint from the given string.
*
* Format should be bracketed array of arrays of floats, like so: [[1.0, 2.2], [3.3, 4.2], ...]
*
*/
128 bool makeFootprintFromString(
const std::string & footprint_string,
std::vector<geometry_msgs::msg::Point> & footprint );
} // end namespace nav2_costmap_2d
#endif // NAV2_COSTMAP_2D__FOOTPRINT_HPP_
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Modified by: Shivang Patel ( shivaang14@gmail.com )
#ifndef NAV2_COSTMAP_2D__FOOTPRINT_COLLISION_CHECKER_HPP_
#define NAV2_COSTMAP_2D__FOOTPRINT_COLLISION_CHECKER_HPP_
#include <string>
#include <vector>
#include <memory>
#include <algorithm>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose2_d.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_util/robot_utils.hpp"
namespace nav2_costmap_2d
{
typedef std::vector<geometry_msgs::msg::Point> Footprint;
/**
* @class FootprintCollisionChecker
* @brief Checker for collision with a footprint on a costmap
*/
template<typename CostmapT>
40 class FootprintCollisionChecker
{
public:
/**
* @brief A constructor.
*/
46 FootprintCollisionChecker( );
/**
* @brief A constructor.
*/
50 explicit FootprintCollisionChecker( CostmapT costmap );
/**
* @brief Find the footprint cost in oriented footprint
*/
54 double footprintCost( const Footprint footprint );
/**
* @brief Find the footprint cost a a post with an unoriented footprint
*/
58 double footprintCostAtPose( double x, double y, double theta, const Footprint footprint );
/**
* @brief Get the cost for a line segment
*/
62 double lineCost( int x0, int x1, int y0, int y1 ) const;
/**
* @brief Get the map coordinates from a world point
*/
66 bool worldToMap( double wx, double wy, unsigned int & mx, unsigned int & my );
/**
* @brief Get the cost of a point
*/
70 double pointCost( int x, int y ) const;
/**
* @brief Set the current costmap object to use for collision detection
*/
74 void setCostmap( CostmapT costmap );
/**
* @brief Get the current costmap object
*/
78 CostmapT getCostmap( )
{
return costmap_;
}
protected:
84 CostmapT costmap_;
};
} // namespace nav2_costmap_2d
#endif // NAV2_COSTMAP_2D__FOOTPRINT_COLLISION_CHECKER_HPP_
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_COSTMAP_2D__FOOTPRINT_SUBSCRIBER_HPP_
#define NAV2_COSTMAP_2D__FOOTPRINT_SUBSCRIBER_HPP_
#include <string>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/footprint.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/robot_utils.hpp"
namespace nav2_costmap_2d
{
/**
* @class FootprintSubscriber
* @brief Subscriber to the footprint topic to get current robot footprint
* ( if changing ) for use in collision avoidance
*/
33 class FootprintSubscriber
{
public:
/**
* @brief A constructor
*/
39 FootprintSubscriber(
40 const nav2_util::LifecycleNode::WeakPtr & parent,
41 const std::string & topic_name,
42 tf2_ros::Buffer & tf,
43 std::string robot_base_frame = "base_link",
double transform_tolerance = 0.1 );
/**
* @brief A constructor
*/
49 FootprintSubscriber(
50 const rclcpp::Node::WeakPtr & parent,
51 const std::string & topic_name,
52 tf2_ros::Buffer & tf,
53 std::string robot_base_frame = "base_link",
double transform_tolerance = 0.1 );
/**
* @brief A destructor
*/
59 ~FootprintSubscriber( ) {}
/**
* @brief Returns the latest robot footprint, in the form as received from topic ( oriented ).
*
* @param footprint Output param. Latest received footprint
* @param footprint_header Output param. Header associated with the footprint
* @return False if no footprint has been received
*/
68 bool getFootprintRaw(
69 std::vector<geometry_msgs::msg::Point> & footprint,
70 std_msgs::msg::Header & footprint_header );
/**
* @brief Returns the latest robot footprint, transformed into robot base frame ( unoriented ).
*
* @param footprint Output param. Latest received footprint, unoriented
* @param footprint_header Output param. Header associated with the footprint
* @return False if no footprint has been received or if transformation failed
*/
79 bool getFootprintInRobotFrame(
80 std::vector<geometry_msgs::msg::Point> & footprint,
81 std_msgs::msg::Header & footprint_header );
protected:
/**
* @brief Callback to process new footprint updates.
*/
87 void footprint_callback( const geometry_msgs::msg::PolygonStamped::SharedPtr msg );
89 tf2_ros::Buffer & tf_;
90 std::string robot_base_frame_;
double transform_tolerance_;
bool footprint_received_{false};
geometry_msgs::msg::PolygonStamped::SharedPtr footprint_;
rclcpp::Subscription<geometry_msgs::msg::PolygonStamped>::SharedPtr footprint_sub_;
};
} // namespace nav2_costmap_2d
#endif // NAV2_COSTMAP_2D__FOOTPRINT_SUBSCRIBER_HPP_
/*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef NAV2_COSTMAP_2D__INFLATION_LAYER_HPP_
#define NAV2_COSTMAP_2D__INFLATION_LAYER_HPP_
#include <map>
#include <vector>
#include <mutex>
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/layer.hpp"
#include "nav2_costmap_2d/layered_costmap.hpp"
namespace nav2_costmap_2d
{
/**
* @class CellData
* @brief Storage for cell information used during obstacle inflation
*/
55 class CellData
{
public:
/**
* @brief Constructor for a CellData objects
* @param i The index of the cell in the cost map
* @param x The x coordinate of the cell in the cost map
* @param y The y coordinate of the cell in the cost map
* @param sx The x coordinate of the closest obstacle cell in the costmap
* @param sy The y coordinate of the closest obstacle cell in the costmap
* @return
*/
67 CellData( double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy )
: index_( static_cast<unsigned int>( i ) ), x_( x ), y_( y ), src_x_( sx ), src_y_( sy )
{
}
unsigned int index_;
unsigned int x_, y_;
unsigned int src_x_, src_y_;
};
/**
* @class InflationLayer
* @brief Layer to convolve costmap by robot's radius or footprint to prevent
* collisions and largely simply collision checking
*/
81 class InflationLayer : public Layer
{
public:
/**
* @brief A constructor
*/
87 InflationLayer( );
/**
* @brief A destructor
*/
92 ~InflationLayer( );
/**
* @brief Initialization process of layer on startup
*/
void onInitialize( ) override;
/**
* @brief Update the bounds of the master costmap by this layer's update dimensions
* @param robot_x X pose of robot
* @param robot_y Y pose of robot
* @param robot_yaw Robot orientation
* @param min_x X min map coord of the window to update
* @param min_y Y min map coord of the window to update
* @param max_x X max map coord of the window to update
* @param max_y Y max map coord of the window to update
*/
void updateBounds(
double robot_x, double robot_y, double robot_yaw, double * min_x,
double * min_y,
double * max_x,
double * max_y ) override;
/**
* @brief Update the costs in the master costmap in the window
* @param master_grid The master costmap grid to update
* @param min_x X min map coord of the window to update
* @param min_y Y min map coord of the window to update
* @param max_x X max map coord of the window to update
* @param max_y Y max map coord of the window to update
*/
void updateCosts(
nav2_costmap_2d::Costmap2D & master_grid,
int min_i, int min_j, int max_i, int max_j ) override;
/**
* @brief Match the size of the master costmap
*/
void matchSize( ) override;
/**
* @brief If clearing operations should be processed on this layer or not
*/
virtual bool isClearable( ) {return false;}
/**
* @brief Reset this costmap
*/
void reset( ) override
{
matchSize( );
142 current_ = false;
}
/** @brief Given a distance, compute a cost.
* @param distance The distance from an obstacle in cells
* @return A cost value for the distance */
148 inline unsigned char computeCost( double distance ) const
{
unsigned char cost = 0;
if ( distance == 0 ) {
cost = LETHAL_OBSTACLE;
} else if ( distance * resolution_ <= inscribed_radius_ ) {
cost = INSCRIBED_INFLATED_OBSTACLE;
} else {
// make sure cost falls off by Euclidean distance
double factor =
exp( -1.0 * cost_scaling_factor_ * ( distance * resolution_ - inscribed_radius_ ) );
cost = static_cast<unsigned char>( ( INSCRIBED_INFLATED_OBSTACLE - 1 ) * factor );
}
return cost;
}
// Provide a typedef to ease future code maintenance
typedef std::recursive_mutex mutex_t;
/**
* @brief Get the mutex of the inflation inforamtion
*/
170 mutex_t * getMutex( )
{
return access_;
}
protected:
/**
* @brief Process updates on footprint changes to the inflation layer
*/
void onFootprintChanged( ) override;
private:
/**
* @brief Lookup pre-computed distances
* @param mx The x coordinate of the current cell
* @param my The y coordinate of the current cell
* @param src_x The x coordinate of the source cell
* @param src_y The y coordinate of the source cell
* @return
*/
inline double distanceLookup(
unsigned int mx, unsigned int my, unsigned int src_x,
unsigned int src_y )
{
unsigned int dx = ( mx > src_x ) ? mx - src_x : src_x - mx;
unsigned int dy = ( my > src_y ) ? my - src_y : src_y - my;
return cached_distances_[dx * cache_length_ + dy];
}
/**
* @brief Lookup pre-computed costs
* @param mx The x coordinate of the current cell
* @param my The y coordinate of the current cell
* @param src_x The x coordinate of the source cell
* @param src_y The y coordinate of the source cell
* @return
*/
inline unsigned char costLookup(
unsigned int mx, unsigned int my, unsigned int src_x,
unsigned int src_y )
{
unsigned int dx = ( mx > src_x ) ? mx - src_x : src_x - mx;
unsigned int dy = ( my > src_y ) ? my - src_y : src_y - my;
return cached_costs_[dx * cache_length_ + dy];
}
/**
* @brief Compute cached dsitances
*/
void computeCaches( );
/**
* @brief Compute cached dsitances
*/
int generateIntegerDistances( );
/**
* @brief Compute cached dsitances
*/
unsigned int cellDistance( double world_dist )
{
return layered_costmap_->getCostmap( )->cellDistance( world_dist );
}
/**
* @brief Enqueue new cells in cache distance update search
*/
inline void enqueue(
unsigned int index, unsigned int mx, unsigned int my,
unsigned int src_x, unsigned int src_y );
/**
* @brief Callback executed when a parameter change is detected
* @param event ParameterEvent message
*/
rcl_interfaces::msg::SetParametersResult
dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters );
double inflation_radius_, inscribed_radius_, cost_scaling_factor_;
bool inflate_unknown_, inflate_around_unknown_;
unsigned int cell_inflation_radius_;
unsigned int cached_cell_inflation_radius_;
std::vector<std::vector<CellData>> inflation_cells_;
double resolution_;
std::vector<bool> seen_;
std::vector<unsigned char> cached_costs_;
std::vector<double> cached_distances_;
std::vector<std::vector<int>> distance_matrix_;
unsigned int cache_length_;
double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
// Indicates that the entire costmap should be reinflated next time around.
bool need_reinflation_;
mutex_t * access_;
// Dynamic parameters handler
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
};
} // namespace nav2_costmap_2d
#endif // NAV2_COSTMAP_2D__INFLATION_LAYER_HPP_
/*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: David V. Lu!!
*********************************************************************/
#ifndef NAV2_COSTMAP_2D__LAYER_HPP_
#define NAV2_COSTMAP_2D__LAYER_HPP_
#include <string>
#include <vector>
#include <unordered_set>
#include "tf2_ros/buffer.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/layered_costmap.hpp"
#include "nav2_util/lifecycle_node.hpp"
namespace nav2_costmap_2d
{
52 class LayeredCostmap;
/**
* @class Layer
* @brief Abstract class for layered costmap plugin implementations
*/
58 class Layer
{
public:
/**
* @brief A constructor
*/
64 Layer( );
/**
* @brief A destructor
*/
68 virtual ~Layer( ) {}
/**
* @brief Initialization process of layer on startup
*/
73 void initialize(
74 LayeredCostmap * parent,
75 std::string name,
76 tf2_ros::Buffer * tf,
77 const nav2_util::LifecycleNode::WeakPtr & node,
78 rclcpp::CallbackGroup::SharedPtr callback_group );
/** @brief Stop publishers. */
80 virtual void deactivate( ) {}
/** @brief Restart publishers if they've been stopped. */
82 virtual void activate( ) {}
/**
* @brief Reset this costmap
*/
86 virtual void reset( ) = 0;
/**
* @brief If clearing operations should be processed on this layer or not
*/
90 virtual bool isClearable( ) = 0;
/**
* @brief This is called by the LayeredCostmap to poll this plugin as to how
* much of the costmap it needs to update. Each layer can increase
* the size of this bounds.
*
* For more details, see "Layered Costmaps for Context-Sensitive Navigation",
* by Lu et. Al, IROS 2014.
*/
100 virtual void updateBounds(
double robot_x, double robot_y, double robot_yaw, double * min_x,
double * min_y,
double * max_x,
double * max_y ) = 0;
/**
* @brief Actually update the underlying costmap, only within the bounds
* calculated during UpdateBounds( ).
*/
110 virtual void updateCosts(
111 Costmap2D & master_grid,
int min_i, int min_j, int max_i, int max_j ) = 0;
/** @brief Implement this to make this layer match the size of the parent costmap. */
115 virtual void matchSize( ) {}
/** @brief LayeredCostmap calls this whenever the footprint there
* changes ( via LayeredCostmap::setFootprint( ) ). Override to be
* notified of changes to the robot's footprint. */
120 virtual void onFootprintChanged( ) {}
/** @brief Get the name of the costmap layer */
122 std::string getName( ) const
{
return name_;
}
/**
* @brief Check to make sure all the data in the layer is up to date.
* If the layer is not up to date, then it may be unsafe to
* plan using the data from this layer, and the planner may
* need to know.
*
* A layer's current state should be managed by the protected
* variable current_.
* @return Whether the data in the layer is up to date.
*/
137 bool isCurrent( ) const
{
return current_;
}
/** @brief Convenience function for layered_costmap_->getFootprint( ). */
143 const std::vector<geometry_msgs::msg::Point> & getFootprint( ) const;
/** @brief Convenience functions for declaring ROS parameters */
146 void declareParameter(
147 const std::string & param_name,
148 const rclcpp::ParameterValue & value );
/** @brief Convenience functions for declaring ROS parameters */
150 void declareParameter(
151 const std::string & param_name,
152 const rclcpp::ParameterType & param_type );
/** @brief Convenience functions for declaring ROS parameters */
154 bool hasParameter( const std::string & param_name );
/** @brief Convenience functions for declaring ROS parameters */
156 std::string getFullName( const std::string & param_name );
protected:
159 LayeredCostmap * layered_costmap_;
160 std::string name_;
161 tf2_ros::Buffer * tf_;
162 rclcpp::CallbackGroup::SharedPtr callback_group_;
163 rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
164 rclcpp::Clock::SharedPtr clock_;
rclcpp::Logger logger_{rclcpp::get_logger( "nav2_costmap_2d" )};
/** @brief This is called at the end of initialize( ). Override to
* implement subclass-specific initialization.
*
* tf_, name_, and layered_costmap_ will all be set already when this is called.
*/
172 virtual void onInitialize( ) {}
bool current_;
// Currently this var is managed by subclasses.
// TODO( bpwilcox ): make this managed by this class and/or container class.
bool enabled_;
// Names of the parameters declared on the ROS node
std::unordered_set<std::string> local_params_;
private:
std::vector<geometry_msgs::msg::Point> footprint_spec_;
};
} // namespace nav2_costmap_2d
#endif // NAV2_COSTMAP_2D__LAYER_HPP_
/*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef NAV2_COSTMAP_2D__LAYERED_COSTMAP_HPP_
#define NAV2_COSTMAP_2D__LAYERED_COSTMAP_HPP_
#include <memory>
#include <string>
#include <vector>
#include "nav2_costmap_2d/cost_values.hpp"
#include "nav2_costmap_2d/layer.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
namespace nav2_costmap_2d
{
51 class Layer;
/**
* @class LayeredCostmap
* @brief Instantiates different layer plugins and aggregates them into one score
*/
57 class LayeredCostmap
{
public:
/**
* @brief Constructor for a costmap
*/
63 LayeredCostmap( std::string global_frame, bool rolling_window, bool track_unknown );
/**
* @brief Destructor
*/
68 ~LayeredCostmap( );
/**
* @brief Update the underlying costmap with new data.
* If you want to update the map outside of the update loop that runs, you can call this.
*/
74 void updateMap( double robot_x, double robot_y, double robot_yaw );
76 std::string getGlobalFrameID( ) const
{
return global_frame_;
}
/**
* @brief Resize the map to a new size, resolution, or origin
*/
84 void resizeMap(
unsigned int size_x, unsigned int size_y, double resolution, double origin_x,
double origin_y,
87 bool size_locked = false );
/**
* @brief Get the size of the bounds for update
*/
92 void getUpdatedBounds( double & minx, double & miny, double & maxx, double & maxy )
{
minx = minx_;
miny = miny_;
maxx = maxx_;
maxy = maxy_;
}
/**
* @brief If the costmap is current, e.g. are all the layers processing recent data
* and not stale information for a good state.
*/
104 bool isCurrent( );
/**
* @brief Get the costmap pointer to the master costmap
*/
109 Costmap2D * getCostmap( )
{
return &combined_costmap_;
}
/**
* @brief If this costmap is rolling or not
*/
117 bool isRolling( )
{
return rolling_window_;
}
/**
* @brief If this costmap is tracking unknown space or not
*/
125 bool isTrackingUnknown( )
{
return combined_costmap_.getDefaultValue( ) == nav2_costmap_2d::NO_INFORMATION;
}
/**
* @brief Get the vector of pointers to the costmap plugins
*/
std::vector<std::shared_ptr<Layer>> * getPlugins( )
{
return &plugins_;
}
/**
* @brief Get the vector of pointers to the costmap filters
*/
141 std::vector<std::shared_ptr<Layer>> * getFilters( )
{
return &filters_;
}
/**
* @brief Add a new plugin to the plugins vector to process
*/
149 void addPlugin( std::shared_ptr<Layer> plugin );
/**
* @brief Add a new costmap filter plugin to the filters vector to process
*/
155 void addFilter( std::shared_ptr<Layer> filter )
{
filters_.push_back( filter );
}
/**
* @brief Get if the size of the costmap is locked
*/
163 bool isSizeLocked( )
{
return size_locked_;
}
/**
* @brief Get the bounds of the costmap
*/
171 void getBounds( unsigned int * x0, unsigned int * xn, unsigned int * y0, unsigned int * yn )
{
*x0 = bx0_;
*xn = bxn_;
*y0 = by0_;
*yn = byn_;
}
/**
* @brief if the costmap is initialized
*/
182 bool isInitialized( )
{
return initialized_;
}
/** @brief Updates the stored footprint, updates the circumscribed
* and inscribed radii, and calls onFootprintChanged( ) in all
* layers. */
190 void setFootprint( const std::vector<geometry_msgs::msg::Point> & footprint_spec );
/** @brief Returns the latest footprint stored with setFootprint( ). */
193 const std::vector<geometry_msgs::msg::Point> & getFootprint( ) {return footprint_;}
/** @brief The radius of a circle centered at the origin of the
* robot which just surrounds all points on the robot's
* footprint.
*
* This is updated by setFootprint( ). */
200 double getCircumscribedRadius( ) {return circumscribed_radius_;}
/** @brief The radius of a circle centered at the origin of the
* robot which is just within all points and edges of the robot's
* footprint.
*
* This is updated by setFootprint( ). */
207 double getInscribedRadius( ) {return inscribed_radius_;}
/** @brief Checks if the robot is outside the bounds of its costmap in the case
* of poorly configured setups. */
211 bool isOutofBounds( double robot_x, double robot_y );
private:
// primary_costmap_ is a bottom costmap used by plugins when costmap filters were enabled.
// combined_costmap_ is a final costmap where all results produced by plugins and filters ( if any )
// to be merged.
// The separation is aimed to avoid interferences of work between plugins and filters.
// primay_costmap_ and combined_costmap_ have the same sizes, origins and default values.
Costmap2D primary_costmap_, combined_costmap_;
std::string global_frame_;
bool rolling_window_; /// < @brief Whether or not the costmap should roll with the robot
bool current_;
double minx_, miny_, maxx_, maxy_;
unsigned int bx0_, bxn_, by0_, byn_;
std::vector<std::shared_ptr<Layer>> plugins_;
std::vector<std::shared_ptr<Layer>> filters_;
bool initialized_;
bool size_locked_;
double circumscribed_radius_, inscribed_radius_;
std::vector<geometry_msgs::msg::Point> footprint_;
};
} // namespace nav2_costmap_2d
#endif // NAV2_COSTMAP_2D__LAYERED_COSTMAP_HPP_
/*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES ( INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE )
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Authors: Conor McGann
*/
#ifndef NAV2_COSTMAP_2D__OBSERVATION_HPP_
#define NAV2_COSTMAP_2D__OBSERVATION_HPP_
#include <geometry_msgs/msg/point.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
namespace nav2_costmap_2d
{
/**
* @brief Stores an observation in terms of a point cloud and the origin of the source
* @note Tried to make members and constructor arguments const but the compiler would not accept the default
* assignment operator for vector insertion!
*/
46 class Observation
{
public:
/**
* @brief Creates an empty observation
*/
Observation( )
: cloud_( new sensor_msgs::msg::PointCloud2( ) ), obstacle_max_range_( 0.0 ), obstacle_min_range_( 0.0 ),
raytrace_max_range_( 0.0 ),
raytrace_min_range_( 0.0 )
{
}
/**
* @brief A destructor
*/
virtual ~Observation( )
{
delete cloud_;
}
/**
* @brief Copy assignment operator
* @param obs The observation to copy
*/
70 Observation & operator=( const Observation & obs )
{
origin_ = obs.origin_;
cloud_ = new sensor_msgs::msg::PointCloud2( *( obs.cloud_ ) );
obstacle_max_range_ = obs.obstacle_max_range_;
obstacle_min_range_ = obs.obstacle_min_range_;
raytrace_max_range_ = obs.raytrace_max_range_;
raytrace_min_range_ = obs.raytrace_min_range_;
return *this;
}
/**
* @brief Creates an observation from an origin point and a point cloud
* @param origin The origin point of the observation
* @param cloud The point cloud of the observation
* @param obstacle_max_range The range out to which an observation should be able to insert obstacles
* @param obstacle_min_range The range from which an observation should be able to insert obstacles
* @param raytrace_max_range The range out to which an observation should be able to clear via raytracing
* @param raytrace_min_range The range from which an observation should be able to clear via raytracing
*/
91 Observation(
geometry_msgs::msg::Point & origin, const sensor_msgs::msg::PointCloud2 & cloud,
double obstacle_max_range, double obstacle_min_range, double raytrace_max_range,
double raytrace_min_range )
: origin_( origin ), cloud_( new sensor_msgs::msg::PointCloud2( cloud ) ),
obstacle_max_range_( obstacle_max_range ), obstacle_min_range_( obstacle_min_range ),
raytrace_max_range_( raytrace_max_range ), raytrace_min_range_(
raytrace_min_range )
{
}
/**
* @brief Copy constructor
* @param obs The observation to copy
*/
Observation( const Observation & obs )
: origin_( obs.origin_ ), cloud_( new sensor_msgs::msg::PointCloud2( *( obs.cloud_ ) ) ),
obstacle_max_range_( obs.obstacle_max_range_ ), obstacle_min_range_( obs.obstacle_min_range_ ),
raytrace_max_range_( obs.raytrace_max_range_ ),
raytrace_min_range_( obs.raytrace_min_range_ )
{
}
/**
* @brief Creates an observation from a point cloud
* @param cloud The point cloud of the observation
* @param obstacle_max_range The range out to which an observation should be able to insert obstacles
* @param obstacle_min_range The range from which an observation should be able to insert obstacles
*/
Observation(
const sensor_msgs::msg::PointCloud2 & cloud, double obstacle_max_range,
double obstacle_min_range )
: cloud_( new sensor_msgs::msg::PointCloud2( cloud ) ), obstacle_max_range_( obstacle_max_range ),
obstacle_min_range_( obstacle_min_range ),
raytrace_max_range_( 0.0 ), raytrace_min_range_( 0.0 )
{
}
geometry_msgs::msg::Point origin_;
sensor_msgs::msg::PointCloud2 * cloud_;
double obstacle_max_range_, obstacle_min_range_, raytrace_max_range_, raytrace_min_range_;
};
} // namespace nav2_costmap_2d
#endif // NAV2_COSTMAP_2D__OBSERVATION_HPP_
/*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#ifndef NAV2_COSTMAP_2D__OBSERVATION_BUFFER_HPP_
#define NAV2_COSTMAP_2D__OBSERVATION_BUFFER_HPP_
#include <vector>
#include <list>
#include <string>
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "rclcpp/time.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_sensor_msgs/tf2_sensor_msgs.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "nav2_costmap_2d/observation.hpp"
#include "nav2_util/lifecycle_node.hpp"
namespace nav2_costmap_2d
{
/**
* @class ObservationBuffer
* @brief Takes in point clouds from sensors, transforms them to the desired frame, and stores them
*/
59 class ObservationBuffer
{
public:
/**
* @brief Constructs an observation buffer
* @param topic_name The topic of the observations, used as an identifier for error and warning messages
* @param observation_keep_time Defines the persistence of observations in seconds, 0 means only keep the latest
* @param expected_update_rate How often this buffer is expected to be updated, 0 means there is no limit
* @param min_obstacle_height The minimum height of a hitpoint to be considered legal
* @param max_obstacle_height The minimum height of a hitpoint to be considered legal
* @param obstacle_max_range The range to which the sensor should be trusted for inserting obstacles
* @param obstacle_min_range The range from which the sensor should be trusted for inserting obstacles
* @param raytrace_max_range The range to which the sensor should be trusted for raytracing to clear out space
* @param raytrace_min_range The range from which the sensor should be trusted for raytracing to clear out space
* @param tf2_buffer A reference to a tf2 Buffer
* @param global_frame The frame to transform PointClouds into
* @param sensor_frame The frame of the origin of the sensor, can be left blank to be read from the messages
* @param tf_tolerance The amount of time to wait for a transform to be available when setting a new global frame
*/
78 ObservationBuffer(
79 const nav2_util::LifecycleNode::WeakPtr & parent,
80 std::string topic_name,
double observation_keep_time,
double expected_update_rate,
double min_obstacle_height, double max_obstacle_height, double obstacle_max_range,
double obstacle_min_range,
85 double raytrace_max_range, double raytrace_min_range, tf2_ros::Buffer & tf2_buffer,
86 std::string global_frame,
87 std::string sensor_frame,
88 tf2::Duration tf_tolerance );
/**
* @brief Destructor... cleans up
*/
93 ~ObservationBuffer( );
/**
* @brief Transforms a PointCloud to the global frame and buffers it
* <b>Note: The burden is on the user to make sure the transform is available... ie they should use a MessageNotifier</b>
* @param cloud The cloud to be buffered
*/
100 void bufferCloud( const sensor_msgs::msg::PointCloud2 & cloud );
/**
* @brief Pushes copies of all current observations onto the end of the vector passed in
* @param observations The vector to be filled
*/
106 void getObservations( std::vector<Observation> & observations );
/**
* @brief Check if the observation buffer is being update at its expected rate
* @return True if it is being updated at the expected rate, false otherwise
*/
112 bool isCurrent( ) const;
/**
* @brief Lock the observation buffer
*/
117 inline void lock( )
{
lock_.lock( );
}
/**
* @brief Lock the observation buffer
*/
125 inline void unlock( )
{
lock_.unlock( );
}
/**
* @brief Reset last updated timestamp
*/
133 void resetLastUpdated( );
private:
/**
* @brief Removes any stale observations from the buffer list
*/
139 void purgeStaleObservations( );
141 rclcpp::Clock::SharedPtr clock_;
rclcpp::Logger logger_{rclcpp::get_logger( "nav2_costmap_2d" )};
tf2_ros::Buffer & tf2_buffer_;
const rclcpp::Duration observation_keep_time_;
const rclcpp::Duration expected_update_rate_;
rclcpp::Time last_updated_;
std::string global_frame_;
std::string sensor_frame_;
std::list<Observation> observation_list_;
std::string topic_name_;
double min_obstacle_height_, max_obstacle_height_;
std::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely
double obstacle_max_range_, obstacle_min_range_, raytrace_max_range_, raytrace_min_range_;
tf2::Duration tf_tolerance_;
};
} // namespace nav2_costmap_2d
#endif // NAV2_COSTMAP_2D__OBSERVATION_BUFFER_HPP_
1 /*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef NAV2_COSTMAP_2D__OBSTACLE_LAYER_HPP_
#define NAV2_COSTMAP_2D__OBSTACLE_LAYER_HPP_
#include <memory>
#include <string>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "laser_geometry/laser_geometry.hpp"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wreorder"
#include "tf2_ros/message_filter.h"
#pragma GCC diagnostic pop
#include "message_filters/subscriber.h"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "sensor_msgs/msg/point_cloud.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "nav2_costmap_2d/costmap_layer.hpp"
#include "nav2_costmap_2d/layered_costmap.hpp"
#include "nav2_costmap_2d/observation_buffer.hpp"
#include "nav2_costmap_2d/footprint.hpp"
namespace nav2_costmap_2d
{
/**
* @class ObstacleLayer
* @brief Takes in laser and pointcloud data to populate into 2D costmap
*/
68 class ObstacleLayer : public CostmapLayer
{
public:
/**
* @brief A constructor
*/
74 ObstacleLayer( )
{
costmap_ = NULL; // this is the unsigned char* member of parent class Costmap2D.
}
/**
* @brief A destructor
*/
82 virtual ~ObstacleLayer( );
/**
* @brief Initialization process of layer on startup
*/
86 virtual void onInitialize( );
/**
* @brief Update the bounds of the master costmap by this layer's update dimensions
* @param robot_x X pose of robot
* @param robot_y Y pose of robot
* @param robot_yaw Robot orientation
* @param min_x X min map coord of the window to update
* @param min_y Y min map coord of the window to update
* @param max_x X max map coord of the window to update
* @param max_y Y max map coord of the window to update
*/
97 virtual void updateBounds(
double robot_x, double robot_y, double robot_yaw, double * min_x,
double * min_y,
double * max_x,
double * max_y );
/**
* @brief Update the costs in the master costmap in the window
* @param master_grid The master costmap grid to update
* @param min_x X min map coord of the window to update
* @param min_y Y min map coord of the window to update
* @param max_x X max map coord of the window to update
* @param max_y Y max map coord of the window to update
*/
110 virtual void updateCosts(
111 nav2_costmap_2d::Costmap2D & master_grid,
int min_i, int min_j, int max_i, int max_j );
/**
* @brief Deactivate the layer
*/
117 virtual void deactivate( );
/**
* @brief Activate the layer
*/
122 virtual void activate( );
/**
* @brief Reset this costmap
*/
127 virtual void reset( );
/**
* @brief If clearing operations should be processed on this layer or not
*/
132 virtual bool isClearable( ) {return true;}
/**
* @brief Callback executed when a parameter change is detected
* @param event ParameterEvent message
*/
rcl_interfaces::msg::SetParametersResult
139 dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters );
/**
* @brief triggers the update of observations buffer
*/
144 void resetBuffersLastUpdated( );
/**
* @brief A callback to handle buffering LaserScan messages
* @param message The message returned from a message notifier
* @param buffer A pointer to the observation buffer to update
*/
151 void laserScanCallback(
152 sensor_msgs::msg::LaserScan::ConstSharedPtr message,
153 const std::shared_ptr<nav2_costmap_2d::ObservationBuffer> & buffer );
/**
* @brief A callback to handle buffering LaserScan messages which need filtering to turn Inf values into range_max.
* @param message The message returned from a message notifier
* @param buffer A pointer to the observation buffer to update
*/
160 void laserScanValidInfCallback(
161 sensor_msgs::msg::LaserScan::ConstSharedPtr message,
162 const std::shared_ptr<nav2_costmap_2d::ObservationBuffer> & buffer );
/**
* @brief A callback to handle buffering PointCloud2 messages
* @param message The message returned from a message notifier
* @param buffer A pointer to the observation buffer to update
*/
169 void pointCloud2Callback(
170 sensor_msgs::msg::PointCloud2::ConstSharedPtr message,
171 const std::shared_ptr<nav2_costmap_2d::ObservationBuffer> & buffer );
// for testing purposes
174 void addStaticObservation( nav2_costmap_2d::Observation & obs, bool marking, bool clearing );
175 void clearStaticObservations( bool marking, bool clearing );
protected:
/**
* @brief Get the observations used to mark space
* @param marking_observations A reference to a vector that will be populated with the observations
* @return True if all the observation buffers are current, false otherwise
*/
183 bool getMarkingObservations(
184 std::vector<nav2_costmap_2d::Observation> & marking_observations ) const;
/**
* @brief Get the observations used to clear space
* @param clearing_observations A reference to a vector that will be populated with the observations
* @return True if all the observation buffers are current, false otherwise
*/
191 bool getClearingObservations(
192 std::vector<nav2_costmap_2d::Observation> & clearing_observations ) const;
/**
* @brief Clear freespace based on one observation
* @param clearing_observation The observation used to raytrace
* @param min_x
* @param min_y
* @param max_x
* @param max_y
*/
202 virtual void raytraceFreespace(
203 const nav2_costmap_2d::Observation & clearing_observation,
double * min_x, double * min_y,
double * max_x,
double * max_y );
/**
* @brief Process update costmap with raytracing the window bounds
*/
211 void updateRaytraceBounds(
double ox, double oy, double wx, double wy, double max_range, double min_range,
double * min_x, double * min_y,
double * max_x,
double * max_y );
217 std::vector<geometry_msgs::msg::Point> transformed_footprint_;
218 bool footprint_clearing_enabled_;
/**
* @brief Clear costmap layer info below the robot's footprint
*/
222 void updateFootprint(
double robot_x, double robot_y, double robot_yaw, double * min_x,
double * min_y,
double * max_x,
double * max_y );
228 std::string global_frame_; ///< @brief The global frame for the costmap
double max_obstacle_height_; ///< @brief Max Obstacle Height
/// @brief Used to project laser scans into point clouds
232 laser_geometry::LaserProjection projector_;
/// @brief Used for the observation message filters
std::vector<std::shared_ptr<message_filters::SubscriberBase<rclcpp_lifecycle::LifecycleNode>>>
observation_subscribers_;
/// @brief Used to make sure that transforms are available for each sensor
std::vector<std::shared_ptr<tf2_ros::MessageFilterBase>> observation_notifiers_;
/// @brief Used to store observations from various sensors
std::vector<std::shared_ptr<nav2_costmap_2d::ObservationBuffer>> observation_buffers_;
/// @brief Used to store observation buffers used for marking obstacles
std::vector<std::shared_ptr<nav2_costmap_2d::ObservationBuffer>> marking_buffers_;
/// @brief Used to store observation buffers used for clearing obstacles
std::vector<std::shared_ptr<nav2_costmap_2d::ObservationBuffer>> clearing_buffers_;
/// @brief Dynamic parameters handler
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
// Used only for testing purposes
std::vector<nav2_costmap_2d::Observation> static_clearing_observations_;
std::vector<nav2_costmap_2d::Observation> static_marking_observations_;
bool rolling_window_;
bool was_reset_;
int combination_method_;
};
} // namespace nav2_costmap_2d
#endif // NAV2_COSTMAP_2D__OBSTACLE_LAYER_HPP_
/*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2018 David V. Lu!!
* Copyright ( c ) 2020, Bytes Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef NAV2_COSTMAP_2D__RANGE_SENSOR_LAYER_HPP_
#define NAV2_COSTMAP_2D__RANGE_SENSOR_LAYER_HPP_
#include <list>
#include <string>
#include <vector>
#include <mutex>
#include "map_msgs/msg/occupancy_grid_update.hpp"
#include "message_filters/subscriber.h"
#include "nav2_costmap_2d/costmap_layer.hpp"
#include "nav2_costmap_2d/layered_costmap.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "sensor_msgs/msg/range.hpp"
namespace nav2_costmap_2d
{
/**
* @class RangeSensorLayer
* @brief Takes in IR/Sonar/similar point measurement sensors and populates in costmap
*/
60 class RangeSensorLayer : public CostmapLayer
{
public:
enum class InputSensorType
{
VARIABLE,
FIXED,
ALL
};
/**
* @brief A constructor
*/
73 RangeSensorLayer( );
/**
* @brief Initialization process of layer on startup
*/
78 virtual void onInitialize( );
/**
* @brief Update the bounds of the master costmap by this layer's update dimensions
* @param robot_x X pose of robot
* @param robot_y Y pose of robot
* @param robot_yaw Robot orientation
* @param min_x X min map coord of the window to update
* @param min_y Y min map coord of the window to update
* @param max_x X max map coord of the window to update
* @param max_y Y max map coord of the window to update
*/
virtual void updateBounds(
double robot_x, double robot_y, double robot_yaw,
double * min_x, double * min_y, double * max_x, double * max_y );
/**
* @brief Update the costs in the master costmap in the window
* @param master_grid The master costmap grid to update
* @param min_x X min map coord of the window to update
* @param min_y Y min map coord of the window to update
* @param max_x X max map coord of the window to update
* @param max_y Y max map coord of the window to update
*/
virtual void updateCosts(
nav2_costmap_2d::Costmap2D & master_grid, int min_i,
int min_j, int max_i, int max_j );
/**
* @brief Reset this costmap
*/
virtual void reset( );
/**
* @brief Deactivate the layer
*/
virtual void deactivate( );
/**
* @brief Activate the layer
*/
virtual void activate( );
/**
* @brief If clearing operations should be processed on this layer or not
*/
virtual bool isClearable( ) {return true;}
/**
* @brief Handle an incoming Range message to populate into costmap
*/
void bufferIncomingRangeMsg( const sensor_msgs::msg::Range::SharedPtr range_message );
private:
/**
* @brief Processes all sensors into the costmap buffered from callbacks
*/
void updateCostmap( );
/**
* @brief Update the actual costmap with the values processed
*/
void updateCostmap( sensor_msgs::msg::Range & range_message, bool clear_sensor_cone );
/**
* @brief Process general incoming range sensor data. If min=max ranges,
* fixed processor callback is used, else uses variable callback
*/
void processRangeMsg( sensor_msgs::msg::Range & range_message );
/**
* @brief Process fixed range incoming range sensor data
*/
void processFixedRangeMsg( sensor_msgs::msg::Range & range_message );
/**
* @brief Process variable range incoming range sensor data
*/
void processVariableRangeMsg( sensor_msgs::msg::Range & range_message );
/**
* @brief Reset the angle min/max x, and min/max y values
*/
void resetRange( );
/**
* @brief Get the gamma value for an angle, theta
*/
inline double gamma( double theta );
/**
* @brief Get the delta value for an angle, phi
*/
inline double delta( double phi );
/**
* @brief Apply the sensor model of the layer for range sensors
*/
inline double sensor_model( double r, double phi, double theta );
/**
* @brief Get angles
*/
inline void get_deltas( double angle, double * dx, double * dy );
/**
* @brief Update the cost in a cell with information
*/
inline void update_cell(
double ox, double oy, double ot,
double r, double nx, double ny, bool clear );
/**
* @brief Find probability value of a cost
*/
inline double to_prob( unsigned char c )
{
return static_cast<double>( c ) / nav2_costmap_2d::LETHAL_OBSTACLE;
}
/**
* @brief Find cost value of a probability
*/
inline unsigned char to_cost( double p )
{
return static_cast<unsigned char>( p * nav2_costmap_2d::LETHAL_OBSTACLE );
}
std::function<void( sensor_msgs::msg::Range & range_message )> processRangeMessageFunc_;
std::mutex range_message_mutex_;
std::list<sensor_msgs::msg::Range> range_msgs_buffer_;
double max_angle_, phi_v_;
double inflate_cone_;
std::string global_frame_;
double clear_threshold_, mark_threshold_;
bool clear_on_max_reading_;
bool was_reset_;
tf2::Duration transform_tolerance_;
double no_readings_timeout_;
rclcpp::Time last_reading_time_;
unsigned int buffered_readings_;
std::vector<rclcpp::Subscription<sensor_msgs::msg::Range>::SharedPtr> range_subs_;
double min_x_, min_y_, max_x_, max_y_;
/**
* @brief Find the area of 3 points of a triangle
*/
float area( int x1, int y1, int x2, int y2, int x3, int y3 )
{
return fabs( ( x1 * ( y2 - y3 ) + x2 * ( y3 - y1 ) + x3 * ( y1 - y2 ) ) / 2.0 );
}
/**
* @brief Find the cross product of 3 vectors, A, B, C
*/
int orient2d( int Ax, int Ay, int Bx, int By, int Cx, int Cy )
{
return ( Bx - Ax ) * ( Cy - Ay ) - ( By - Ay ) * ( Cx - Ax );
}
};
} // namespace nav2_costmap_2d
#endif // NAV2_COSTMAP_2D__RANGE_SENSOR_LAYER_HPP_
/*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef NAV2_COSTMAP_2D__STATIC_LAYER_HPP_
#define NAV2_COSTMAP_2D__STATIC_LAYER_HPP_
#include <mutex>
#include <string>
#include <vector>
#include "map_msgs/msg/occupancy_grid_update.hpp"
#include "message_filters/subscriber.h"
#include "nav2_costmap_2d/costmap_layer.hpp"
#include "nav2_costmap_2d/layered_costmap.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "rclcpp/rclcpp.hpp"
namespace nav2_costmap_2d
{
/**
* @class StaticLayer
* @brief Takes in a map generated from SLAM to add costs to costmap
*/
59 class StaticLayer : public CostmapLayer
{
public:
/**
* @brief Static Layer constructor
*/
65 StaticLayer( );
/**
* @brief Static Layer destructor
*/
69 virtual ~StaticLayer( );
/**
* @brief Initialization process of layer on startup
*/
74 virtual void onInitialize( );
/**
* @brief Activate this layer
*/
79 virtual void activate( );
/**
* @brief Deactivate this layer
*/
83 virtual void deactivate( );
/**
* @brief Reset this costmap
*/
88 virtual void reset( );
/**
* @brief If clearing operations should be processed on this layer or not
*/
93 virtual bool isClearable( ) {return false;}
/**
* @brief Update the bounds of the master costmap by this layer's update dimensions
* @param robot_x X pose of robot
* @param robot_y Y pose of robot
* @param robot_yaw Robot orientation
* @param min_x X min map coord of the window to update
* @param min_y Y min map coord of the window to update
* @param max_x X max map coord of the window to update
* @param max_y Y max map coord of the window to update
*/
105 virtual void updateBounds(
double robot_x, double robot_y, double robot_yaw, double * min_x,
double * min_y, double * max_x, double * max_y );
/**
* @brief Update the costs in the master costmap in the window
* @param master_grid The master costmap grid to update
* @param min_x X min map coord of the window to update
* @param min_y Y min map coord of the window to update
* @param max_x X max map coord of the window to update
* @param max_y Y max map coord of the window to update
*/
117 virtual void updateCosts(
118 nav2_costmap_2d::Costmap2D & master_grid,
int min_i, int min_j, int max_i, int max_j );
/**
* @brief Match the size of the master costmap
*/
124 virtual void matchSize( );
private:
/**
* @brief Get parameters of layer
*/
130 void getParameters( );
/**
* @brief Process a new map coming from a topic
*/
135 void processMap( const nav_msgs::msg::OccupancyGrid & new_map );
/**
* @brief Callback to update the costmap's map from the map_server
* @param new_map The map to put into the costmap. The origin of the new
* map along with its size will determine what parts of the costmap's
* static map are overwritten.
*/
143 void incomingMap( const nav_msgs::msg::OccupancyGrid::SharedPtr new_map );
/**
* @brief Callback to update the costmap's map from the map_server ( or SLAM )
* with an update in a particular area of the map
*/
148 void incomingUpdate( map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr update );
/**
* @brief Interpret the value in the static map given on the topic to
* convert into costs for the costmap to utilize
*/
154 unsigned char interpretValue( unsigned char value );
/**
* @brief Callback executed when a parameter change is detected
* @param event ParameterEvent message
*/
rcl_interfaces::msg::SetParametersResult
161 dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters );
163 std::string global_frame_; ///< @brief The global frame for the costmap
164 std::string map_frame_; /// @brief frame that map is located in
bool has_updated_data_{false};
unsigned int x_{0};
unsigned int y_{0};
unsigned int width_{0};
unsigned int height_{0};
rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_sub_;
rclcpp::Subscription<map_msgs::msg::OccupancyGridUpdate>::SharedPtr map_update_sub_;
// Parameters
std::string map_topic_;
bool map_subscribe_transient_local_;
bool subscribe_to_updates_;
bool track_unknown_space_;
bool use_maximum_;
unsigned char lethal_threshold_;
unsigned char unknown_cost_value_;
bool trinary_costmap_;
bool map_received_{false};
tf2::Duration transform_tolerance_;
std::atomic<bool> update_in_progress_;
nav_msgs::msg::OccupancyGrid::SharedPtr map_buffer_;
// Dynamic parameters handler
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
};
} // namespace nav2_costmap_2d
#endif // NAV2_COSTMAP_2D__STATIC_LAYER_HPP_
1 /*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef NAV2_COSTMAP_2D__VOXEL_LAYER_HPP_
#define NAV2_COSTMAP_2D__VOXEL_LAYER_HPP_
#include <vector>
#include "message_filters/subscriber.h"
#include <rclcpp/rclcpp.hpp>
#include <nav2_costmap_2d/layer.hpp>
#include <nav2_costmap_2d/layered_costmap.hpp>
#include <nav2_costmap_2d/observation_buffer.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <nav2_msgs/msg/voxel_grid.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <laser_geometry/laser_geometry.hpp>
#include <sensor_msgs/msg/point_cloud.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <nav2_costmap_2d/obstacle_layer.hpp>
#include <nav2_voxel_grid/voxel_grid.hpp>
namespace nav2_costmap_2d
{
/**
* @class VoxelLayer
* @brief Takes laser and pointcloud data to populate a 3D voxel representation of the environment
*/
64 class VoxelLayer : public ObstacleLayer
{
public:
/**
* @brief Voxel Layer constructor
*/
70 VoxelLayer( )
: voxel_grid_( 0, 0, 0 )
{
costmap_ = NULL; // this is the unsigned char* member of parent class's parent class Costmap2D
}
/**
* @brief Voxel Layer destructor
*/
79 virtual ~VoxelLayer( );
/**
* @brief Initialization process of layer on startup
*/
84 virtual void onInitialize( );
/**
* @brief Update the bounds of the master costmap by this layer's update dimensions
* @param robot_x X pose of robot
* @param robot_y Y pose of robot
* @param robot_yaw Robot orientation
* @param min_x X min map coord of the window to update
* @param min_y Y min map coord of the window to update
* @param max_x X max map coord of the window to update
* @param max_y Y max map coord of the window to update
*/
96 virtual void updateBounds(
double robot_x, double robot_y, double robot_yaw, double * min_x,
double * min_y,
double * max_x,
double * max_y );
/**
* @brief Update the layer's origin to a new pose, often when in a rolling costmap
*/
105 void updateOrigin( double new_origin_x, double new_origin_y );
/**
* @brief If layer is discretely populated
*/
110 bool isDiscretized( )
{
return true;
}
/**
* @brief Match the size of the master costmap
*/
118 virtual void matchSize( );
/**
* @brief Reset this costmap
*/
123 virtual void reset( );
/**
* @brief If clearing operations should be processed on this layer or not
*/
128 virtual bool isClearable( ) {return true;}
protected:
/**
* @brief Reset internal maps
*/
134 virtual void resetMaps( );
private:
/**
* @brief Use raycasting between 2 points to clear freespace
*/
140 virtual void raytraceFreespace(
141 const nav2_costmap_2d::Observation & clearing_observation,
double * min_x, double * min_y,
double * max_x,
double * max_y );
146 bool publish_voxel_;
147 rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::VoxelGrid>::SharedPtr voxel_pub_;
148 nav2_voxel_grid::VoxelGrid voxel_grid_;
double z_resolution_, origin_z_;
int unknown_threshold_, mark_threshold_, size_z_;
151 rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::PointCloud2>::SharedPtr
clearing_endpoints_pub_;
/**
* @brief Covert world coordinates into map coordinates
*/
157 inline bool worldToMap3DFloat(
double wx, double wy, double wz, double & mx, double & my,
double & mz )
{
if ( wx < origin_x_ || wy < origin_y_ || wz < origin_z_ ) {
return false;
}
mx = ( ( wx - origin_x_ ) / resolution_ );
my = ( ( wy - origin_y_ ) / resolution_ );
mz = ( ( wz - origin_z_ ) / z_resolution_ );
if ( mx < size_x_ && my < size_y_ && mz < size_z_ ) {
return true;
}
return false;
}
/**
* @brief Covert world coordinates into map coordinates
*/
177 inline bool worldToMap3D(
double wx, double wy, double wz, unsigned int & mx, unsigned int & my,
unsigned int & mz )
{
if ( wx < origin_x_ || wy < origin_y_ || wz < origin_z_ ) {
return false;
}
mx = static_cast<unsigned int>( ( wx - origin_x_ ) / resolution_ );
my = static_cast<unsigned int>( ( wy - origin_y_ ) / resolution_ );
mz = static_cast<unsigned int>( ( wz - origin_z_ ) / z_resolution_ );
if ( mx < size_x_ && my < size_y_ && mz < ( unsigned int )size_z_ ) {
return true;
}
return false;
}
/**
* @brief Covert map coordinates into world coordinates
*/
199 inline void mapToWorld3D(
unsigned int mx, unsigned int my, unsigned int mz, double & wx,
double & wy,
double & wz )
{
// returns the center point of the cell
wx = origin_x_ + ( mx + 0.5 ) * resolution_;
wy = origin_y_ + ( my + 0.5 ) * resolution_;
wz = origin_z_ + ( mz + 0.5 ) * z_resolution_;
}
/**
* @brief Find L2 norm distance in 3D
*/
213 inline double dist( double x0, double y0, double z0, double x1, double y1, double z1 )
{
return sqrt( ( x1 - x0 ) * ( x1 - x0 ) + ( y1 - y0 ) * ( y1 - y0 ) + ( z1 - z0 ) * ( z1 - z0 ) );
}
/**
* @brief Get the height of the voxel sizes in meters
*/
221 double getSizeInMetersZ( ) const
{
return ( size_z_ - 1 + 0.5 ) * z_resolution_;
}
/**
* @brief Callback executed when a parameter change is detected
* @param event ParameterEvent message
*/
rcl_interfaces::msg::SetParametersResult
231 dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters );
// Dynamic parameters handler
234 rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
};
} // namespace nav2_costmap_2d
#endif // NAV2_COSTMAP_2D__VOXEL_LAYER_HPP_
1 /*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2020 Samsung Research Russia
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the <ORGANIZATION> nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Alexey Merzlyakov
*********************************************************************/
#include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp"
#include <exception>
namespace nav2_costmap_2d
{
45 CostmapFilter::CostmapFilter( )
: filter_info_topic_( "" ), mask_topic_( "" )
{
access_ = new mutex_t( );
}
51 CostmapFilter::~CostmapFilter( )
{
delete access_;
}
56 void CostmapFilter::onInitialize( )
{
rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
try {
// Declare common for all costmap filters parameters
declareParameter( "enabled", rclcpp::ParameterValue( true ) );
declareParameter( "filter_info_topic", rclcpp::PARAMETER_STRING );
declareParameter( "transform_tolerance", rclcpp::ParameterValue( 0.1 ) );
// Get parameters
node->get_parameter( name_ + "." + "enabled", enabled_ );
filter_info_topic_ = node->get_parameter( name_ + "." + "filter_info_topic" ).as_string( );
double transform_tolerance;
node->get_parameter( name_ + "." + "transform_tolerance", transform_tolerance );
transform_tolerance_ = tf2::durationFromSec( transform_tolerance );
} catch ( const std::exception & ex ) {
RCLCPP_ERROR( logger_, "Parameter problem: %s", ex.what( ) );
throw ex;
}
}
81 void CostmapFilter::activate( )
{
initializeFilter( filter_info_topic_ );
}
86 void CostmapFilter::deactivate( )
{
resetFilter( );
}
91 void CostmapFilter::reset( )
{
resetFilter( );
initializeFilter( filter_info_topic_ );
current_ = false;
}
98 void CostmapFilter::updateBounds(
double robot_x, double robot_y, double robot_yaw,
double * /*min_x*/, double * /*min_y*/, double * /*max_x*/, double * /*max_y*/ )
{
if ( !enabled_ ) {
return;
}
latest_pose_.x = robot_x;
latest_pose_.y = robot_y;
latest_pose_.theta = robot_yaw;
}
111 void CostmapFilter::updateCosts(
112 nav2_costmap_2d::Costmap2D & master_grid,
int min_i, int min_j, int max_i, int max_j )
{
if ( !enabled_ ) {
return;
}
process( master_grid, min_i, min_j, max_i, max_j, latest_pose_ );
current_ = true;
}
} // namespace nav2_costmap_2d
1 /*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2020 Samsung Research Russia
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the <ORGANIZATION> nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Alexey Merzlyakov
*********************************************************************/
#include <string>
#include <memory>
#include <algorithm>
#include "tf2/convert.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "nav2_costmap_2d/costmap_filters/keepout_filter.hpp"
#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
namespace nav2_costmap_2d
{
50 KeepoutFilter::KeepoutFilter( )
: filter_info_sub_( nullptr ), mask_sub_( nullptr ), mask_costmap_( nullptr ),
mask_frame_( "" ), global_frame_( "" )
{
}
56 void KeepoutFilter::initializeFilter(
57 const std::string & filter_info_topic )
{
std::lock_guard<CostmapFilter::mutex_t> guard( *getMutex( ) );
rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
filter_info_topic_ = filter_info_topic;
// Setting new costmap filter info subscriber
RCLCPP_INFO(
logger_,
"KeepoutFilter: Subscribing to \"%s\" topic for filter info...",
filter_info_topic_.c_str( ) );
filter_info_sub_ = node->create_subscription<nav2_msgs::msg::CostmapFilterInfo>(
filter_info_topic_, rclcpp::QoS( rclcpp::KeepLast( 1 ) ).transient_local( ).reliable( ),
std::bind( &KeepoutFilter::filterInfoCallback, this, std::placeholders::_1 ) );
global_frame_ = layered_costmap_->getGlobalFrameID( );
}
79 void KeepoutFilter::filterInfoCallback(
80 const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg )
{
std::lock_guard<CostmapFilter::mutex_t> guard( *getMutex( ) );
rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
if ( !mask_sub_ ) {
RCLCPP_INFO(
logger_,
"KeepoutFilter: Received filter info from %s topic.", filter_info_topic_.c_str( ) );
} else {
RCLCPP_WARN(
logger_,
"KeepoutFilter: New costmap filter info arrived from %s topic. Updating old filter info.",
filter_info_topic_.c_str( ) );
// Resetting previous subscriber each time when new costmap filter information arrives
mask_sub_.reset( );
}
// Checking that base and multiplier are set to their default values
if ( msg->base != BASE_DEFAULT || msg->multiplier != MULTIPLIER_DEFAULT ) {
RCLCPP_ERROR(
logger_,
"KeepoutFilter: For proper use of keepout filter base and multiplier"
" in CostmapFilterInfo message should be set to their default values ( %f and %f )",
BASE_DEFAULT, MULTIPLIER_DEFAULT );
}
mask_topic_ = msg->filter_mask_topic;
// Setting new filter mask subscriber
RCLCPP_INFO(
logger_,
"KeepoutFilter: Subscribing to \"%s\" topic for filter mask...",
mask_topic_.c_str( ) );
mask_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
mask_topic_, rclcpp::QoS( rclcpp::KeepLast( 1 ) ).transient_local( ).reliable( ),
std::bind( &KeepoutFilter::maskCallback, this, std::placeholders::_1 ) );
}
123 void KeepoutFilter::maskCallback(
124 const nav_msgs::msg::OccupancyGrid::SharedPtr msg )
{
std::lock_guard<CostmapFilter::mutex_t> guard( *getMutex( ) );
rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
if ( !mask_costmap_ ) {
RCLCPP_INFO(
logger_,
"KeepoutFilter: Received filter mask from %s topic.", mask_topic_.c_str( ) );
} else {
RCLCPP_WARN(
logger_,
"KeepoutFilter: New filter mask arrived from %s topic. Updating old filter mask.",
mask_topic_.c_str( ) );
mask_costmap_.reset( );
}
// Making a new mask_costmap_
mask_costmap_ = std::make_unique<Costmap2D>( *msg );
mask_frame_ = msg->header.frame_id;
}
150 void KeepoutFilter::process(
151 nav2_costmap_2d::Costmap2D & master_grid,
int min_i, int min_j, int max_i, int max_j,
153 const geometry_msgs::msg::Pose2D & /*pose*/ )
{
std::lock_guard<CostmapFilter::mutex_t> guard( *getMutex( ) );
if ( !mask_costmap_ ) {
// Show warning message every 2 seconds to not litter an output
RCLCPP_WARN_THROTTLE(
logger_, *( clock_ ), 2000,
"KeepoutFilter: Filter mask was not received" );
return;
}
tf2::Transform tf2_transform;
tf2_transform.setIdentity( ); // initialize by identical transform
int mg_min_x, mg_min_y; // masger_grid indexes of bottom-left window corner
int mg_max_x, mg_max_y; // masger_grid indexes of top-right window corner
if ( mask_frame_ != global_frame_ ) {
// Filter mask and current layer are in different frames:
// prepare frame transformation if mask_frame_ != global_frame_
geometry_msgs::msg::TransformStamped transform;
try {
transform = tf_->lookupTransform(
mask_frame_, global_frame_, tf2::TimePointZero,
transform_tolerance_ );
} catch ( tf2::TransformException & ex ) {
RCLCPP_ERROR(
logger_,
"KeepoutFilter: Failed to get costmap frame ( %s ) "
"transformation to mask frame ( %s ) with error: %s",
global_frame_.c_str( ), mask_frame_.c_str( ), ex.what( ) );
return;
}
tf2::fromMsg( transform.transform, tf2_transform );
mg_min_x = min_i;
mg_min_y = min_j;
mg_max_x = max_i;
mg_max_y = max_j;
} else {
// Filter mask and current layer are in the same frame:
// apply the following optimization - iterate only in overlapped
// ( min_i, min_j )..( max_i, max_j ) & mask_costmap_ area.
//
// mask_costmap_
// *----------------------------*
// | |
// | |
// | ( 2 ) |
// *-----+-------* |
// | |///////|<- overlapped area |
// | |///////| to iterate in |
// | *-------+--------------------*
// | ( 1 ) |
// | |
// *-------------*
// master_grid ( min_i, min_j )..( max_i, max_j ) window
//
// ToDo: after costmap rotation will be added, this should be re-worked.
double wx, wy; // world coordinates
// Calculating bounds corresponding to bottom-left overlapping ( 1 ) corner
// mask_costmap_ -> master_grid intexes conversion
const double half_cell_size = 0.5 * mask_costmap_->getResolution( );
wx = mask_costmap_->getOriginX( ) + half_cell_size;
wy = mask_costmap_->getOriginY( ) + half_cell_size;
master_grid.worldToMapNoBounds( wx, wy, mg_min_x, mg_min_y );
// Calculation of ( 1 ) corner bounds
if ( mg_min_x >= max_i || mg_min_y >= max_j ) {
// There is no overlapping. Do nothing.
return;
}
mg_min_x = std::max( min_i, mg_min_x );
mg_min_y = std::max( min_j, mg_min_y );
// Calculating bounds corresponding to top-right window ( 2 ) corner
// mask_costmap_ -> master_grid intexes conversion
wx = mask_costmap_->getOriginX( ) +
mask_costmap_->getSizeInCellsX( ) * mask_costmap_->getResolution( ) + half_cell_size;
wy = mask_costmap_->getOriginY( ) +
mask_costmap_->getSizeInCellsY( ) * mask_costmap_->getResolution( ) + half_cell_size;
master_grid.worldToMapNoBounds( wx, wy, mg_max_x, mg_max_y );
// Calculation of ( 2 ) corner bounds
if ( mg_max_x <= min_i || mg_max_y <= min_j ) {
// There is no overlapping. Do nothing.
return;
}
mg_max_x = std::min( max_i, mg_max_x );
mg_max_y = std::min( max_j, mg_max_y );
}
// unsigned<-signed conversions.
unsigned const int mg_min_x_u = static_cast<unsigned int>( mg_min_x );
unsigned const int mg_min_y_u = static_cast<unsigned int>( mg_min_y );
unsigned const int mg_max_x_u = static_cast<unsigned int>( mg_max_x );
unsigned const int mg_max_y_u = static_cast<unsigned int>( mg_max_y );
unsigned int i, j; // master_grid iterators
unsigned int index; // corresponding index of master_grid
double gl_wx, gl_wy; // world coordinates in a global_frame_
double msk_wx, msk_wy; // world coordinates in a mask_frame_
unsigned int mx, my; // mask_costmap_ coordinates
unsigned char data, old_data; // master_grid element data
// Main master_grid updating loop
// Iterate in costmap window by master_grid indexes
unsigned char * master_array = master_grid.getCharMap( );
for ( i = mg_min_x_u; i < mg_max_x_u; i++ ) {
for ( j = mg_min_y_u; j < mg_max_y_u; j++ ) {
index = master_grid.getIndex( i, j );
old_data = master_array[index];
// Calculating corresponding to ( i, j ) point at mask_costmap_:
// Get world coordinates in global_frame_
master_grid.mapToWorld( i, j, gl_wx, gl_wy );
if ( mask_frame_ != global_frame_ ) {
// Transform ( i, j ) point from global_frame_ to mask_frame_
tf2::Vector3 point( gl_wx, gl_wy, 0 );
point = tf2_transform * point;
msk_wx = point.x( );
msk_wy = point.y( );
} else {
// In this case master_grid and filter-mask are in the same frame
msk_wx = gl_wx;
msk_wy = gl_wy;
}
// Get mask coordinates corresponding to ( i, j ) point at mask_costmap_
if ( mask_costmap_->worldToMap( msk_wx, msk_wy, mx, my ) ) {
data = mask_costmap_->getCost( mx, my );
// Update if mask_ data is valid and greater than existing master_grid's one
if ( data == NO_INFORMATION ) {
continue;
}
if ( data > old_data || old_data == NO_INFORMATION ) {
master_array[index] = data;
}
}
}
}
}
294 void KeepoutFilter::resetFilter( )
{
std::lock_guard<CostmapFilter::mutex_t> guard( *getMutex( ) );
filter_info_sub_.reset( );
mask_sub_.reset( );
}
302 bool KeepoutFilter::isActive( )
{
std::lock_guard<CostmapFilter::mutex_t> guard( *getMutex( ) );
if ( mask_costmap_ ) {
return true;
}
return false;
}
} // namespace nav2_costmap_2d
#include "pluginlib/class_list_macros.hpp"
315 PLUGINLIB_EXPORT_CLASS( nav2_costmap_2d::KeepoutFilter, nav2_costmap_2d::Layer )
1 /*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2020 Samsung Research Russia
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the <ORGANIZATION> nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Alexey Merzlyakov
*********************************************************************/
#include "nav2_costmap_2d/costmap_filters/speed_filter.hpp"
#include <cmath>
#include <utility>
#include <memory>
#include <string>
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"
#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
namespace nav2_costmap_2d
{
52 SpeedFilter::SpeedFilter( )
: filter_info_sub_( nullptr ), mask_sub_( nullptr ),
speed_limit_pub_( nullptr ), filter_mask_( nullptr ), mask_frame_( "" ), global_frame_( "" ),
speed_limit_( NO_SPEED_LIMIT ), speed_limit_prev_( NO_SPEED_LIMIT )
{
}
59 void SpeedFilter::initializeFilter(
60 const std::string & filter_info_topic )
{
std::lock_guard<CostmapFilter::mutex_t> guard( *getMutex( ) );
rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
// Declare "speed_limit_topic" parameter specific to SpeedFilter only
std::string speed_limit_topic;
declareParameter( "speed_limit_topic", rclcpp::ParameterValue( "speed_limit" ) );
node->get_parameter( name_ + "." + "speed_limit_topic", speed_limit_topic );
filter_info_topic_ = filter_info_topic;
// Setting new costmap filter info subscriber
RCLCPP_INFO(
logger_,
"SpeedFilter: Subscribing to \"%s\" topic for filter info...",
filter_info_topic_.c_str( ) );
filter_info_sub_ = node->create_subscription<nav2_msgs::msg::CostmapFilterInfo>(
filter_info_topic_, rclcpp::QoS( rclcpp::KeepLast( 1 ) ).transient_local( ).reliable( ),
std::bind( &SpeedFilter::filterInfoCallback, this, std::placeholders::_1 ) );
// Get global frame required for speed limit publisher
global_frame_ = layered_costmap_->getGlobalFrameID( );
// Create new speed limit publisher
speed_limit_pub_ = node->create_publisher<nav2_msgs::msg::SpeedLimit>(
speed_limit_topic, rclcpp::QoS( 10 ) );
speed_limit_pub_->on_activate( );
// Reset speed conversion states
base_ = BASE_DEFAULT;
multiplier_ = MULTIPLIER_DEFAULT;
percentage_ = false;
}
98 void SpeedFilter::filterInfoCallback(
99 const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg )
{
std::lock_guard<CostmapFilter::mutex_t> guard( *getMutex( ) );
rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
if ( !mask_sub_ ) {
RCLCPP_INFO(
logger_,
"SpeedFilter: Received filter info from %s topic.", filter_info_topic_.c_str( ) );
} else {
RCLCPP_WARN(
logger_,
"SpeedFilter: New costmap filter info arrived from %s topic. Updating old filter info.",
filter_info_topic_.c_str( ) );
// Resetting previous subscriber each time when new costmap filter information arrives
mask_sub_.reset( );
}
// Set base_/multiplier_ or use speed limit in % of maximum speed
base_ = msg->base;
multiplier_ = msg->multiplier;
if ( msg->type == SPEED_FILTER_PERCENT ) {
// Using speed limit in % of maximum speed
percentage_ = true;
RCLCPP_INFO(
logger_,
"SpeedFilter: Using expressed in a percent from maximum speed"
"speed_limit = %f + filter_mask_data * %f",
base_, multiplier_ );
} else if ( msg->type == SPEED_FILTER_ABSOLUTE ) {
// Using speed limit in m/s
percentage_ = false;
RCLCPP_INFO(
logger_,
"SpeedFilter: Using absolute speed_limit = %f + filter_mask_data * %f",
base_, multiplier_ );
} else {
RCLCPP_ERROR( logger_, "SpeedFilter: Mode is not supported" );
return;
}
mask_topic_ = msg->filter_mask_topic;
// Setting new filter mask subscriber
RCLCPP_INFO(
logger_,
"SpeedFilter: Subscribing to \"%s\" topic for filter mask...",
mask_topic_.c_str( ) );
mask_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
mask_topic_, rclcpp::QoS( rclcpp::KeepLast( 1 ) ).transient_local( ).reliable( ),
std::bind( &SpeedFilter::maskCallback, this, std::placeholders::_1 ) );
}
156 void SpeedFilter::maskCallback(
157 const nav_msgs::msg::OccupancyGrid::SharedPtr msg )
{
std::lock_guard<CostmapFilter::mutex_t> guard( *getMutex( ) );
if ( !filter_mask_ ) {
RCLCPP_INFO(
logger_,
"SpeedFilter: Received filter mask from %s topic.", mask_topic_.c_str( ) );
} else {
RCLCPP_WARN(
logger_,
"SpeedFilter: New filter mask arrived from %s topic. Updating old filter mask.",
mask_topic_.c_str( ) );
filter_mask_.reset( );
}
filter_mask_ = msg;
mask_frame_ = msg->header.frame_id;
}
177 bool SpeedFilter::transformPose(
178 const geometry_msgs::msg::Pose2D & pose,
179 geometry_msgs::msg::Pose2D & mask_pose ) const
{
if ( mask_frame_ != global_frame_ ) {
// Filter mask and current layer are in different frames:
// Transform ( pose.x, pose.y ) point from current layer frame ( global_frame_ )
// to mask_pose point in mask_frame_
geometry_msgs::msg::TransformStamped transform;
geometry_msgs::msg::PointStamped in, out;
in.header.stamp = clock_->now( );
in.header.frame_id = global_frame_;
in.point.x = pose.x;
in.point.y = pose.y;
in.point.z = 0;
try {
tf_->transform( in, out, mask_frame_, transform_tolerance_ );
} catch ( tf2::TransformException & ex ) {
RCLCPP_ERROR(
logger_,
"SpeedFilter: failed to get costmap frame ( %s ) "
"transformation to mask frame ( %s ) with error: %s",
global_frame_.c_str( ), mask_frame_.c_str( ), ex.what( ) );
return false;
}
mask_pose.x = out.point.x;
mask_pose.y = out.point.y;
} else {
// Filter mask and current layer are in the same frame:
// Just use pose coordinates
mask_pose = pose;
}
return true;
}
214 bool SpeedFilter::worldToMask( double wx, double wy, unsigned int & mx, unsigned int & my ) const
{
double origin_x = filter_mask_->info.origin.position.x;
double origin_y = filter_mask_->info.origin.position.y;
double resolution = filter_mask_->info.resolution;
unsigned int size_x = filter_mask_->info.width;
unsigned int size_y = filter_mask_->info.height;
if ( wx < origin_x || wy < origin_y ) {
return false;
}
mx = std::round( ( wx - origin_x ) / resolution );
my = std::round( ( wy - origin_y ) / resolution );
if ( mx >= size_x || my >= size_y ) {
return false;
}
return true;
}
235 inline int8_t SpeedFilter::getMaskData(
const unsigned int mx, const unsigned int my ) const
{
return filter_mask_->data[my * filter_mask_->info.width + mx];
}
241 void SpeedFilter::process(
242 nav2_costmap_2d::Costmap2D & /*master_grid*/,
int /*min_i*/, int /*min_j*/, int /*max_i*/, int /*max_j*/,
244 const geometry_msgs::msg::Pose2D & pose )
{
std::lock_guard<CostmapFilter::mutex_t> guard( *getMutex( ) );
if ( !filter_mask_ ) {
// Show warning message every 2 seconds to not litter an output
RCLCPP_WARN_THROTTLE(
logger_, *( clock_ ), 2000,
"SpeedFilter: Filter mask was not received" );
return;
}
geometry_msgs::msg::Pose2D mask_pose; // robot coordinates in mask frame
// Transforming robot pose from current layer frame to mask frame
if ( !transformPose( pose, mask_pose ) ) {
return;
}
// Converting mask_pose robot position to filter_mask_ indexes ( mask_robot_i, mask_robot_j )
unsigned int mask_robot_i, mask_robot_j;
if ( !worldToMask( mask_pose.x, mask_pose.y, mask_robot_i, mask_robot_j ) ) {
return;
}
// Getting filter_mask data from cell where the robot placed and
// calculating speed limit value
int8_t speed_mask_data = getMaskData( mask_robot_i, mask_robot_j );
if ( speed_mask_data == SPEED_MASK_NO_LIMIT ) {
// Corresponding filter mask cell is free.
// Setting no speed limit there.
speed_limit_ = NO_SPEED_LIMIT;
} else if ( speed_mask_data == SPEED_MASK_UNKNOWN ) {
// Corresponding filter mask cell is unknown.
// Do nothing.
RCLCPP_ERROR(
logger_,
"SpeedFilter: Found unknown cell in filter_mask[%i, %i], "
"which is invalid for this kind of filter",
mask_robot_i, mask_robot_j );
return;
} else {
// Normal case: speed_mask_data in range of [1..100]
speed_limit_ = speed_mask_data * multiplier_ + base_;
if ( percentage_ ) {
if ( speed_limit_ < 0.0 || speed_limit_ > 100.0 ) {
RCLCPP_WARN(
logger_,
"SpeedFilter: Speed limit in filter_mask[%i, %i] is %f%%, "
"out of bounds of [0, 100]. Setting it to no-limit value.",
mask_robot_i, mask_robot_j, speed_limit_ );
speed_limit_ = NO_SPEED_LIMIT;
}
} else {
if ( speed_limit_ < 0.0 ) {
RCLCPP_WARN(
logger_,
"SpeedFilter: Speed limit in filter_mask[%i, %i] is less than 0 m/s, "
"which can not be true. Setting it to no-limit value.",
mask_robot_i, mask_robot_j );
speed_limit_ = NO_SPEED_LIMIT;
}
}
}
if ( speed_limit_ != speed_limit_prev_ ) {
if ( speed_limit_ != NO_SPEED_LIMIT ) {
RCLCPP_DEBUG( logger_, "SpeedFilter: Speed limit is set to %f", speed_limit_ );
} else {
RCLCPP_DEBUG( logger_, "SpeedFilter: Speed limit is set to its default value" );
}
// Forming and publishing new SpeedLimit message
std::unique_ptr<nav2_msgs::msg::SpeedLimit> msg =
std::make_unique<nav2_msgs::msg::SpeedLimit>( );
msg->header.frame_id = global_frame_;
msg->header.stamp = clock_->now( );
msg->percentage = percentage_;
msg->speed_limit = speed_limit_;
speed_limit_pub_->publish( std::move( msg ) );
speed_limit_prev_ = speed_limit_;
}
}
329 void SpeedFilter::resetFilter( )
{
std::lock_guard<CostmapFilter::mutex_t> guard( *getMutex( ) );
filter_info_sub_.reset( );
mask_sub_.reset( );
if ( speed_limit_pub_ ) {
speed_limit_pub_->on_deactivate( );
speed_limit_pub_.reset( );
}
}
341 bool SpeedFilter::isActive( )
{
std::lock_guard<CostmapFilter::mutex_t> guard( *getMutex( ) );
if ( filter_mask_ ) {
return true;
}
return false;
}
} // namespace nav2_costmap_2d
#include "pluginlib/class_list_macros.hpp"
354 PLUGINLIB_EXPORT_CLASS( nav2_costmap_2d::SpeedFilter, nav2_costmap_2d::Layer )
/*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include "nav2_costmap_2d/inflation_layer.hpp"
#include <limits>
#include <map>
#include <vector>
#include <algorithm>
#include <utility>
#include "nav2_costmap_2d/costmap_math.hpp"
#include "nav2_costmap_2d/footprint.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "rclcpp/parameter_events_filter.hpp"
51 PLUGINLIB_EXPORT_CLASS( nav2_costmap_2d::InflationLayer, nav2_costmap_2d::Layer )
using nav2_costmap_2d::LETHAL_OBSTACLE;
using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
using nav2_costmap_2d::NO_INFORMATION;
using rcl_interfaces::msg::ParameterType;
namespace nav2_costmap_2d
{
61 InflationLayer::InflationLayer( )
: inflation_radius_( 0 ),
inscribed_radius_( 0 ),
cost_scaling_factor_( 0 ),
inflate_unknown_( false ),
inflate_around_unknown_( false ),
cell_inflation_radius_( 0 ),
cached_cell_inflation_radius_( 0 ),
resolution_( 0 ),
cache_length_( 0 ),
last_min_x_( std::numeric_limits<double>::lowest( ) ),
last_min_y_( std::numeric_limits<double>::lowest( ) ),
last_max_x_( std::numeric_limits<double>::max( ) ),
last_max_y_( std::numeric_limits<double>::max( ) )
{
access_ = new mutex_t( );
}
InflationLayer::~InflationLayer( )
{
dyn_params_handler_.reset( );
delete access_;
}
void
InflationLayer::onInitialize( )
{
declareParameter( "enabled", rclcpp::ParameterValue( true ) );
declareParameter( "inflation_radius", rclcpp::ParameterValue( 0.55 ) );
declareParameter( "cost_scaling_factor", rclcpp::ParameterValue( 10.0 ) );
declareParameter( "inflate_unknown", rclcpp::ParameterValue( false ) );
declareParameter( "inflate_around_unknown", rclcpp::ParameterValue( false ) );
{
auto node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
node->get_parameter( name_ + "." + "enabled", enabled_ );
node->get_parameter( name_ + "." + "inflation_radius", inflation_radius_ );
node->get_parameter( name_ + "." + "cost_scaling_factor", cost_scaling_factor_ );
node->get_parameter( name_ + "." + "inflate_unknown", inflate_unknown_ );
node->get_parameter( name_ + "." + "inflate_around_unknown", inflate_around_unknown_ );
dyn_params_handler_ = node->add_on_set_parameters_callback(
std::bind(
&InflationLayer::dynamicParametersCallback,
this, std::placeholders::_1 ) );
}
current_ = true;
seen_.clear( );
cached_distances_.clear( );
cached_costs_.clear( );
need_reinflation_ = false;
cell_inflation_radius_ = cellDistance( inflation_radius_ );
matchSize( );
}
void
InflationLayer::matchSize( )
{
std::lock_guard<Costmap2D::mutex_t> guard( *getMutex( ) );
nav2_costmap_2d::Costmap2D * costmap = layered_costmap_->getCostmap( );
resolution_ = costmap->getResolution( );
cell_inflation_radius_ = cellDistance( inflation_radius_ );
computeCaches( );
seen_ = std::vector<bool>( costmap->getSizeInCellsX( ) * costmap->getSizeInCellsY( ), false );
}
void
InflationLayer::updateBounds(
double /*robot_x*/, double /*robot_y*/, double /*robot_yaw*/, double * min_x,
double * min_y, double * max_x, double * max_y )
{
std::lock_guard<Costmap2D::mutex_t> guard( *getMutex( ) );
if ( need_reinflation_ ) {
last_min_x_ = *min_x;
last_min_y_ = *min_y;
last_max_x_ = *max_x;
last_max_y_ = *max_y;
*min_x = std::numeric_limits<double>::lowest( );
*min_y = std::numeric_limits<double>::lowest( );
*max_x = std::numeric_limits<double>::max( );
*max_y = std::numeric_limits<double>::max( );
need_reinflation_ = false;
} else {
double tmp_min_x = last_min_x_;
double tmp_min_y = last_min_y_;
double tmp_max_x = last_max_x_;
double tmp_max_y = last_max_y_;
last_min_x_ = *min_x;
last_min_y_ = *min_y;
last_max_x_ = *max_x;
last_max_y_ = *max_y;
*min_x = std::min( tmp_min_x, *min_x ) - inflation_radius_;
*min_y = std::min( tmp_min_y, *min_y ) - inflation_radius_;
*max_x = std::max( tmp_max_x, *max_x ) + inflation_radius_;
*max_y = std::max( tmp_max_y, *max_y ) + inflation_radius_;
}
}
void
InflationLayer::onFootprintChanged( )
{
std::lock_guard<Costmap2D::mutex_t> guard( *getMutex( ) );
inscribed_radius_ = layered_costmap_->getInscribedRadius( );
cell_inflation_radius_ = cellDistance( inflation_radius_ );
computeCaches( );
need_reinflation_ = true;
RCLCPP_DEBUG(
logger_, "InflationLayer::onFootprintChanged( ): num footprint points: %zu, "
" inscribed_radius_ = %.3f, inflation_radius_ = %.3f",
layered_costmap_->getFootprint( ).size( ), inscribed_radius_, inflation_radius_ );
}
void
InflationLayer::updateCosts(
nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j,
int max_i,
int max_j )
{
std::lock_guard<Costmap2D::mutex_t> guard( *getMutex( ) );
if ( !enabled_ || ( cell_inflation_radius_ == 0 ) ) {
return;
}
// make sure the inflation list is empty at the beginning of the cycle ( should always be true )
for ( auto & dist : inflation_cells_ ) {
RCLCPP_FATAL_EXPRESSION(
logger_,
!dist.empty( ), "The inflation list must be empty at the beginning of inflation" );
}
unsigned char * master_array = master_grid.getCharMap( );
unsigned int size_x = master_grid.getSizeInCellsX( ), size_y = master_grid.getSizeInCellsY( );
if ( seen_.size( ) != size_x * size_y ) {
RCLCPP_WARN(
logger_, "InflationLayer::updateCosts( ): seen_ vector size is wrong" );
seen_ = std::vector<bool>( size_x * size_y, false );
}
std::fill( begin( seen_ ), end( seen_ ), false );
// We need to include in the inflation cells outside the bounding
// box min_i...max_j, by the amount cell_inflation_radius_. Cells
// up to that distance outside the box can still influence the costs
// stored in cells inside the box.
const int base_min_i = min_i;
const int base_min_j = min_j;
const int base_max_i = max_i;
const int base_max_j = max_j;
min_i -= static_cast<int>( cell_inflation_radius_ );
min_j -= static_cast<int>( cell_inflation_radius_ );
max_i += static_cast<int>( cell_inflation_radius_ );
max_j += static_cast<int>( cell_inflation_radius_ );
min_i = std::max( 0, min_i );
min_j = std::max( 0, min_j );
max_i = std::min( static_cast<int>( size_x ), max_i );
max_j = std::min( static_cast<int>( size_y ), max_j );
// Inflation list; we append cells to visit in a list associated with
// its distance to the nearest obstacle
// We use a map<distance, list> to emulate the priority queue used before,
// with a notable performance boost
// Start with lethal obstacles: by definition distance is 0.0
auto & obs_bin = inflation_cells_[0];
for ( int j = min_j; j < max_j; j++ ) {
for ( int i = min_i; i < max_i; i++ ) {
int index = static_cast<int>( master_grid.getIndex( i, j ) );
unsigned char cost = master_array[index];
if ( cost == LETHAL_OBSTACLE || ( inflate_around_unknown_ && cost == NO_INFORMATION ) ) {
obs_bin.emplace_back( index, i, j, i, j );
}
}
}
// Process cells by increasing distance; new cells are appended to the
// corresponding distance bin, so they
// can overtake previously inserted but farther away cells
for ( const auto & dist_bin : inflation_cells_ ) {
for ( std::size_t i = 0; i < dist_bin.size( ); ++i ) {
// Do not use iterator or for-range based loops to
// iterate though dist_bin, since it's size might
// change when a new cell is enqueued, invalidating all iterators
unsigned int index = dist_bin[i].index_;
// ignore if already visited
if ( seen_[index] ) {
continue;
}
seen_[index] = true;
unsigned int mx = dist_bin[i].x_;
unsigned int my = dist_bin[i].y_;
unsigned int sx = dist_bin[i].src_x_;
unsigned int sy = dist_bin[i].src_y_;
// assign the cost associated with the distance from an obstacle to the cell
unsigned char cost = costLookup( mx, my, sx, sy );
unsigned char old_cost = master_array[index];
// In order to avoid artifacts appeared out of boundary areas
// when some layer is going after inflation_layer,
// we need to apply inflation_layer only to inside of given bounds
if ( static_cast<int>( mx ) >= base_min_i &&
static_cast<int>( my ) >= base_min_j &&
static_cast<int>( mx ) < base_max_i &&
static_cast<int>( my ) < base_max_j )
{
if ( old_cost == NO_INFORMATION &&
( inflate_unknown_ ? ( cost > FREE_SPACE ) : ( cost >= INSCRIBED_INFLATED_OBSTACLE ) ) )
{
master_array[index] = cost;
} else {
master_array[index] = std::max( old_cost, cost );
}
}
// attempt to put the neighbors of the current cell onto the inflation list
if ( mx > 0 ) {
enqueue( index - 1, mx - 1, my, sx, sy );
}
if ( my > 0 ) {
enqueue( index - size_x, mx, my - 1, sx, sy );
}
if ( mx < size_x - 1 ) {
enqueue( index + 1, mx + 1, my, sx, sy );
}
if ( my < size_y - 1 ) {
enqueue( index + size_x, mx, my + 1, sx, sy );
}
}
}
for ( auto & dist : inflation_cells_ ) {
dist.clear( );
dist.reserve( 200 );
}
current_ = true;
}
/**
* @brief Given an index of a cell in the costmap, place it into a list pending for obstacle inflation
* @param grid The costmap
* @param index The index of the cell
* @param mx The x coordinate of the cell ( can be computed from the index, but saves time to store it )
* @param my The y coordinate of the cell ( can be computed from the index, but saves time to store it )
* @param src_x The x index of the obstacle point inflation started at
* @param src_y The y index of the obstacle point inflation started at
*/
void
InflationLayer::enqueue(
unsigned int index, unsigned int mx, unsigned int my,
unsigned int src_x, unsigned int src_y )
{
if ( !seen_[index] ) {
// we compute our distance table one cell further than the
// inflation radius dictates so we can make the check below
double distance = distanceLookup( mx, my, src_x, src_y );
// we only want to put the cell in the list if it is within
// the inflation radius of the obstacle point
if ( distance > cell_inflation_radius_ ) {
return;
}
const unsigned int r = cell_inflation_radius_ + 2;
// push the cell data onto the inflation list and mark
inflation_cells_[distance_matrix_[mx - src_x + r][my - src_y + r]].emplace_back(
index, mx, my, src_x, src_y );
}
}
void
InflationLayer::computeCaches( )
{
std::lock_guard<Costmap2D::mutex_t> guard( *getMutex( ) );
if ( cell_inflation_radius_ == 0 ) {
return;
}
cache_length_ = cell_inflation_radius_ + 2;
// based on the inflation radius... compute distance and cost caches
if ( cell_inflation_radius_ != cached_cell_inflation_radius_ ) {
cached_costs_.resize( cache_length_ * cache_length_ );
cached_distances_.resize( cache_length_ * cache_length_ );
for ( unsigned int i = 0; i < cache_length_; ++i ) {
for ( unsigned int j = 0; j < cache_length_; ++j ) {
cached_distances_[i * cache_length_ + j] = hypot( i, j );
}
}
cached_cell_inflation_radius_ = cell_inflation_radius_;
}
for ( unsigned int i = 0; i < cache_length_; ++i ) {
for ( unsigned int j = 0; j < cache_length_; ++j ) {
cached_costs_[i * cache_length_ + j] = computeCost( cached_distances_[i * cache_length_ + j] );
}
}
int max_dist = generateIntegerDistances( );
inflation_cells_.clear( );
inflation_cells_.resize( max_dist + 1 );
for ( auto & dist : inflation_cells_ ) {
dist.reserve( 200 );
}
}
int
InflationLayer::generateIntegerDistances( )
{
const int r = cell_inflation_radius_ + 2;
const int size = r * 2 + 1;
std::vector<std::pair<int, int>> points;
for ( int y = -r; y <= r; y++ ) {
for ( int x = -r; x <= r; x++ ) {
if ( x * x + y * y <= r * r ) {
points.emplace_back( x, y );
}
}
}
std::sort(
points.begin( ), points.end( ),
[]( const std::pair<int, int> & a, const std::pair<int, int> & b ) -> bool {
return a.first * a.first + a.second * a.second < b.first * b.first + b.second * b.second;
}
);
std::vector<std::vector<int>> distance_matrix( size, std::vector<int>( size, 0 ) );
std::pair<int, int> last = {0, 0};
int level = 0;
for ( auto const & p : points ) {
if ( p.first * p.first + p.second * p.second !=
last.first * last.first + last.second * last.second )
{
level++;
}
distance_matrix[p.first + r][p.second + r] = level;
last = p;
}
distance_matrix_ = distance_matrix;
return level;
}
/**
* @brief Callback executed when a parameter change is detected
* @param event ParameterEvent message
*/
rcl_interfaces::msg::SetParametersResult
InflationLayer::dynamicParametersCallback(
std::vector<rclcpp::Parameter> parameters )
{
std::lock_guard<Costmap2D::mutex_t> guard( *getMutex( ) );
rcl_interfaces::msg::SetParametersResult result;
bool need_cache_recompute = false;
for ( auto parameter : parameters ) {
const auto & param_type = parameter.get_type( );
const auto & param_name = parameter.get_name( );
if ( param_type == ParameterType::PARAMETER_DOUBLE ) {
if ( param_name == name_ + "." + "inflation_radius" &&
inflation_radius_ != parameter.as_double( ) )
{
inflation_radius_ = parameter.as_double( );
need_reinflation_ = true;
need_cache_recompute = true;
} else if ( param_name == name_ + "." + "cost_scaling_factor" && // NOLINT
cost_scaling_factor_ != parameter.as_double( ) )
{
cost_scaling_factor_ = parameter.as_double( );
need_reinflation_ = true;
need_cache_recompute = true;
}
} else if ( param_type == ParameterType::PARAMETER_BOOL ) {
if ( param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool( ) ) {
enabled_ = parameter.as_bool( );
need_reinflation_ = true;
current_ = false;
} else if ( param_name == name_ + "." + "inflate_unknown" && // NOLINT
inflate_unknown_ != parameter.as_bool( ) )
{
inflate_unknown_ = parameter.as_bool( );
need_reinflation_ = true;
} else if ( param_name == name_ + "." + "inflate_around_unknown" && // NOLINT
inflate_around_unknown_ != parameter.as_bool( ) )
{
inflate_around_unknown_ = parameter.as_bool( );
need_reinflation_ = true;
}
}
}
if ( need_cache_recompute ) {
matchSize( );
}
result.successful = true;
return result;
}
} // namespace nav2_costmap_2d
1 /*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
* Steve Macenski
*********************************************************************/
#include "nav2_costmap_2d/obstacle_layer.hpp"
#include <algorithm>
#include <memory>
#include <string>
#include <vector>
#include "pluginlib/class_list_macros.hpp"
#include "sensor_msgs/point_cloud2_iterator.hpp"
#include "nav2_costmap_2d/costmap_math.hpp"
50 PLUGINLIB_EXPORT_CLASS( nav2_costmap_2d::ObstacleLayer, nav2_costmap_2d::Layer )
using nav2_costmap_2d::NO_INFORMATION;
using nav2_costmap_2d::LETHAL_OBSTACLE;
using nav2_costmap_2d::FREE_SPACE;
using nav2_costmap_2d::ObservationBuffer;
using nav2_costmap_2d::Observation;
using rcl_interfaces::msg::ParameterType;
namespace nav2_costmap_2d
{
63 ObstacleLayer::~ObstacleLayer( )
{
dyn_params_handler_.reset( );
for ( auto & notifier : observation_notifiers_ ) {
notifier.reset( );
}
}
71 void ObstacleLayer::onInitialize( )
{
bool track_unknown_space;
double transform_tolerance;
// The topics that we'll subscribe to from the parameter server
std::string topics_string;
declareParameter( "enabled", rclcpp::ParameterValue( true ) );
declareParameter( "footprint_clearing_enabled", rclcpp::ParameterValue( true ) );
declareParameter( "max_obstacle_height", rclcpp::ParameterValue( 2.0 ) );
declareParameter( "combination_method", rclcpp::ParameterValue( 1 ) );
declareParameter( "observation_sources", rclcpp::ParameterValue( std::string( "" ) ) );
auto node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
node->get_parameter( name_ + "." + "enabled", enabled_ );
node->get_parameter( name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_ );
node->get_parameter( name_ + "." + "max_obstacle_height", max_obstacle_height_ );
node->get_parameter( name_ + "." + "combination_method", combination_method_ );
node->get_parameter( "track_unknown_space", track_unknown_space );
node->get_parameter( "transform_tolerance", transform_tolerance );
node->get_parameter( name_ + "." + "observation_sources", topics_string );
dyn_params_handler_ = node->add_on_set_parameters_callback(
std::bind(
&ObstacleLayer::dynamicParametersCallback,
this,
std::placeholders::_1 ) );
RCLCPP_INFO(
logger_,
"Subscribed to Topics: %s", topics_string.c_str( ) );
rolling_window_ = layered_costmap_->isRolling( );
if ( track_unknown_space ) {
default_value_ = NO_INFORMATION;
} else {
default_value_ = FREE_SPACE;
}
ObstacleLayer::matchSize( );
current_ = true;
was_reset_ = false;
global_frame_ = layered_costmap_->getGlobalFrameID( );
auto sub_opt = rclcpp::SubscriptionOptions( );
sub_opt.callback_group = callback_group_;
// now we need to split the topics based on whitespace which we can use a stringstream for
std::stringstream ss( topics_string );
std::string source;
while ( ss >> source ) {
// get the parameters for the specific topic
double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height;
std::string topic, sensor_frame, data_type;
bool inf_is_valid, clearing, marking;
declareParameter( source + "." + "topic", rclcpp::ParameterValue( source ) );
declareParameter( source + "." + "sensor_frame", rclcpp::ParameterValue( std::string( "" ) ) );
declareParameter( source + "." + "observation_persistence", rclcpp::ParameterValue( 0.0 ) );
declareParameter( source + "." + "expected_update_rate", rclcpp::ParameterValue( 0.0 ) );
declareParameter( source + "." + "data_type", rclcpp::ParameterValue( std::string( "LaserScan" ) ) );
declareParameter( source + "." + "min_obstacle_height", rclcpp::ParameterValue( 0.0 ) );
declareParameter( source + "." + "max_obstacle_height", rclcpp::ParameterValue( 0.0 ) );
declareParameter( source + "." + "inf_is_valid", rclcpp::ParameterValue( false ) );
declareParameter( source + "." + "marking", rclcpp::ParameterValue( true ) );
declareParameter( source + "." + "clearing", rclcpp::ParameterValue( false ) );
declareParameter( source + "." + "obstacle_max_range", rclcpp::ParameterValue( 2.5 ) );
declareParameter( source + "." + "obstacle_min_range", rclcpp::ParameterValue( 0.0 ) );
declareParameter( source + "." + "raytrace_max_range", rclcpp::ParameterValue( 3.0 ) );
declareParameter( source + "." + "raytrace_min_range", rclcpp::ParameterValue( 0.0 ) );
node->get_parameter( name_ + "." + source + "." + "topic", topic );
node->get_parameter( name_ + "." + source + "." + "sensor_frame", sensor_frame );
node->get_parameter(
name_ + "." + source + "." + "observation_persistence",
observation_keep_time );
node->get_parameter(
name_ + "." + source + "." + "expected_update_rate",
expected_update_rate );
node->get_parameter( name_ + "." + source + "." + "data_type", data_type );
node->get_parameter( name_ + "." + source + "." + "min_obstacle_height", min_obstacle_height );
node->get_parameter( name_ + "." + source + "." + "max_obstacle_height", max_obstacle_height );
node->get_parameter( name_ + "." + source + "." + "inf_is_valid", inf_is_valid );
node->get_parameter( name_ + "." + source + "." + "marking", marking );
node->get_parameter( name_ + "." + source + "." + "clearing", clearing );
if ( !( data_type == "PointCloud2" || data_type == "LaserScan" ) ) {
RCLCPP_FATAL(
logger_,
"Only topics that use point cloud2s or laser scans are currently supported" );
throw std::runtime_error(
"Only topics that use point cloud2s or laser scans are currently supported" );
}
// get the obstacle range for the sensor
double obstacle_max_range, obstacle_min_range;
node->get_parameter( name_ + "." + source + "." + "obstacle_max_range", obstacle_max_range );
node->get_parameter( name_ + "." + source + "." + "obstacle_min_range", obstacle_min_range );
// get the raytrace ranges for the sensor
double raytrace_max_range, raytrace_min_range;
node->get_parameter( name_ + "." + source + "." + "raytrace_min_range", raytrace_min_range );
node->get_parameter( name_ + "." + source + "." + "raytrace_max_range", raytrace_max_range );
RCLCPP_DEBUG(
logger_,
"Creating an observation buffer for source %s, topic %s, frame %s",
source.c_str( ), topic.c_str( ),
sensor_frame.c_str( ) );
// create an observation buffer
observation_buffers_.push_back(
std::shared_ptr<ObservationBuffer
>(
new ObservationBuffer(
node, topic, observation_keep_time, expected_update_rate,
min_obstacle_height,
max_obstacle_height, obstacle_max_range, obstacle_min_range, raytrace_max_range,
raytrace_min_range, *tf_,
global_frame_,
sensor_frame, tf2::durationFromSec( transform_tolerance ) ) ) );
// check if we'll add this buffer to our marking observation buffers
if ( marking ) {
marking_buffers_.push_back( observation_buffers_.back( ) );
}
// check if we'll also add this buffer to our clearing observation buffers
if ( clearing ) {
clearing_buffers_.push_back( observation_buffers_.back( ) );
}
RCLCPP_DEBUG(
logger_,
"Created an observation buffer for source %s, topic %s, global frame: %s, "
"expected update rate: %.2f, observation persistence: %.2f",
source.c_str( ), topic.c_str( ),
global_frame_.c_str( ), expected_update_rate, observation_keep_time );
rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_sensor_data;
custom_qos_profile.depth = 50;
// create a callback for the topic
if ( data_type == "LaserScan" ) {
auto sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::LaserScan,
rclcpp_lifecycle::LifecycleNode>>( node, topic, custom_qos_profile, sub_opt );
sub->unsubscribe( );
auto filter = std::make_shared<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>>(
*sub, *tf_, global_frame_, 50,
node->get_node_logging_interface( ),
node->get_node_clock_interface( ),
tf2::durationFromSec( transform_tolerance ) );
if ( inf_is_valid ) {
filter->registerCallback(
std::bind(
&ObstacleLayer::laserScanValidInfCallback, this, std::placeholders::_1,
observation_buffers_.back( ) ) );
} else {
filter->registerCallback(
std::bind(
&ObstacleLayer::laserScanCallback, this, std::placeholders::_1,
observation_buffers_.back( ) ) );
}
observation_subscribers_.push_back( sub );
observation_notifiers_.push_back( filter );
observation_notifiers_.back( )->setTolerance( rclcpp::Duration::from_seconds( 0.05 ) );
} else {
auto sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::PointCloud2,
rclcpp_lifecycle::LifecycleNode>>( node, topic, custom_qos_profile, sub_opt );
sub->unsubscribe( );
if ( inf_is_valid ) {
RCLCPP_WARN(
logger_,
"obstacle_layer: inf_is_valid option is not applicable to PointCloud observations." );
}
auto filter = std::make_shared<tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>>(
*sub, *tf_, global_frame_, 50,
node->get_node_logging_interface( ),
node->get_node_clock_interface( ),
tf2::durationFromSec( transform_tolerance ) );
filter->registerCallback(
std::bind(
&ObstacleLayer::pointCloud2Callback, this, std::placeholders::_1,
observation_buffers_.back( ) ) );
observation_subscribers_.push_back( sub );
observation_notifiers_.push_back( filter );
}
if ( sensor_frame != "" ) {
std::vector<std::string> target_frames;
target_frames.push_back( global_frame_ );
target_frames.push_back( sensor_frame );
observation_notifiers_.back( )->setTargetFrames( target_frames );
}
}
}
rcl_interfaces::msg::SetParametersResult
288 ObstacleLayer::dynamicParametersCallback(
289 std::vector<rclcpp::Parameter> parameters )
{
std::lock_guard<Costmap2D::mutex_t> guard( *getMutex( ) );
rcl_interfaces::msg::SetParametersResult result;
for ( auto parameter : parameters ) {
const auto & param_type = parameter.get_type( );
const auto & param_name = parameter.get_name( );
if ( param_type == ParameterType::PARAMETER_DOUBLE ) {
if ( param_name == name_ + "." + "max_obstacle_height" ) {
max_obstacle_height_ = parameter.as_double( );
}
} else if ( param_type == ParameterType::PARAMETER_BOOL ) {
if ( param_name == name_ + "." + "enabled" ) {
enabled_ = parameter.as_bool( );
} else if ( param_name == name_ + "." + "footprint_clearing_enabled" ) {
footprint_clearing_enabled_ = parameter.as_bool( );
}
} else if ( param_type == ParameterType::PARAMETER_INTEGER ) {
if ( param_name == name_ + "." + "combination_method" ) {
combination_method_ = parameter.as_int( );
}
}
}
result.successful = true;
return result;
}
void
320 ObstacleLayer::laserScanCallback(
321 sensor_msgs::msg::LaserScan::ConstSharedPtr message,
322 const std::shared_ptr<nav2_costmap_2d::ObservationBuffer> & buffer )
{
// project the laser into a point cloud
sensor_msgs::msg::PointCloud2 cloud;
cloud.header = message->header;
// project the scan into a point cloud
try {
projector_.transformLaserScanToPointCloud( message->header.frame_id, *message, cloud, *tf_ );
} catch ( tf2::TransformException & ex ) {
RCLCPP_WARN(
logger_,
"High fidelity enabled, but TF returned a transform exception to frame %s: %s",
global_frame_.c_str( ),
ex.what( ) );
projector_.projectLaser( *message, cloud );
} catch ( std::runtime_error & ex ) {
RCLCPP_WARN(
logger_,
"transformLaserScanToPointCloud error, it seems the message from laser is malformed."
" Ignore this message. what( ): %s",
ex.what( ) );
return;
}
// buffer the point cloud
buffer->lock( );
buffer->bufferCloud( cloud );
buffer->unlock( );
}
void
354 ObstacleLayer::laserScanValidInfCallback(
355 sensor_msgs::msg::LaserScan::ConstSharedPtr raw_message,
356 const std::shared_ptr<nav2_costmap_2d::ObservationBuffer> & buffer )
{
// Filter positive infinities ( "Inf"s ) to max_range.
float epsilon = 0.0001; // a tenth of a millimeter
sensor_msgs::msg::LaserScan message = *raw_message;
for ( size_t i = 0; i < message.ranges.size( ); i++ ) {
float range = message.ranges[i];
if ( !std::isfinite( range ) && range > 0 ) {
message.ranges[i] = message.range_max - epsilon;
}
}
// project the laser into a point cloud
sensor_msgs::msg::PointCloud2 cloud;
cloud.header = message.header;
// project the scan into a point cloud
try {
projector_.transformLaserScanToPointCloud( message.header.frame_id, message, cloud, *tf_ );
} catch ( tf2::TransformException & ex ) {
RCLCPP_WARN(
logger_,
"High fidelity enabled, but TF returned a transform exception to frame %s: %s",
global_frame_.c_str( ), ex.what( ) );
projector_.projectLaser( message, cloud );
} catch ( std::runtime_error & ex ) {
RCLCPP_WARN(
logger_,
"transformLaserScanToPointCloud error, it seems the message from laser is malformed."
" Ignore this message. what( ): %s",
ex.what( ) );
return;
}
// buffer the point cloud
buffer->lock( );
buffer->bufferCloud( cloud );
buffer->unlock( );
}
void
397 ObstacleLayer::pointCloud2Callback(
398 sensor_msgs::msg::PointCloud2::ConstSharedPtr message,
399 const std::shared_ptr<ObservationBuffer> & buffer )
{
// buffer the point cloud
buffer->lock( );
buffer->bufferCloud( *message );
buffer->unlock( );
}
void
408 ObstacleLayer::updateBounds(
double robot_x, double robot_y, double robot_yaw, double * min_x,
double * min_y, double * max_x, double * max_y )
{
std::lock_guard<Costmap2D::mutex_t> guard( *getMutex( ) );
if ( rolling_window_ ) {
updateOrigin( robot_x - getSizeInMetersX( ) / 2, robot_y - getSizeInMetersY( ) / 2 );
}
if ( !enabled_ ) {
return;
}
useExtraBounds( min_x, min_y, max_x, max_y );
bool current = true;
std::vector<Observation> observations, clearing_observations;
// get the marking observations
current = current && getMarkingObservations( observations );
// get the clearing observations
current = current && getClearingObservations( clearing_observations );
// update the global current status
current_ = current;
// raytrace freespace
for ( unsigned int i = 0; i < clearing_observations.size( ); ++i ) {
raytraceFreespace( clearing_observations[i], min_x, min_y, max_x, max_y );
}
// place the new obstacles into a priority queue... each with a priority of zero to begin with
for ( std::vector<Observation>::const_iterator it = observations.begin( );
it != observations.end( ); ++it )
{
const Observation & obs = *it;
const sensor_msgs::msg::PointCloud2 & cloud = *( obs.cloud_ );
double sq_obstacle_max_range = obs.obstacle_max_range_ * obs.obstacle_max_range_;
double sq_obstacle_min_range = obs.obstacle_min_range_ * obs.obstacle_min_range_;
sensor_msgs::PointCloud2ConstIterator<float> iter_x( cloud, "x" );
sensor_msgs::PointCloud2ConstIterator<float> iter_y( cloud, "y" );
sensor_msgs::PointCloud2ConstIterator<float> iter_z( cloud, "z" );
for ( ; iter_x != iter_x.end( ); ++iter_x, ++iter_y, ++iter_z ) {
double px = *iter_x, py = *iter_y, pz = *iter_z;
// if the obstacle is too high or too far away from the robot we won't add it
if ( pz > max_obstacle_height_ ) {
RCLCPP_DEBUG( logger_, "The point is too high" );
continue;
}
// compute the squared distance from the hitpoint to the pointcloud's origin
double sq_dist =
( px -
obs.origin_.x ) * ( px - obs.origin_.x ) + ( py - obs.origin_.y ) * ( py - obs.origin_.y ) +
( pz - obs.origin_.z ) * ( pz - obs.origin_.z );
// if the point is far enough away... we won't consider it
if ( sq_dist >= sq_obstacle_max_range ) {
RCLCPP_DEBUG( logger_, "The point is too far away" );
continue;
}
// if the point is too close, do not conisder it
if ( sq_dist < sq_obstacle_min_range ) {
RCLCPP_DEBUG( logger_, "The point is too close" );
continue;
}
// now we need to compute the map coordinates for the observation
unsigned int mx, my;
if ( !worldToMap( px, py, mx, my ) ) {
RCLCPP_DEBUG( logger_, "Computing map coords failed" );
continue;
}
unsigned int index = getIndex( mx, my );
costmap_[index] = LETHAL_OBSTACLE;
touch( px, py, min_x, min_y, max_x, max_y );
}
}
updateFootprint( robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y );
}
void
497 ObstacleLayer::updateFootprint(
double robot_x, double robot_y, double robot_yaw,
double * min_x, double * min_y,
double * max_x,
double * max_y )
{
if ( !footprint_clearing_enabled_ ) {return;}
transformFootprint( robot_x, robot_y, robot_yaw, getFootprint( ), transformed_footprint_ );
for ( unsigned int i = 0; i < transformed_footprint_.size( ); i++ ) {
touch( transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y );
}
}
void
512 ObstacleLayer::updateCosts(
513 nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j,
int max_i,
int max_j )
{
std::lock_guard<Costmap2D::mutex_t> guard( *getMutex( ) );
if ( !enabled_ ) {
return;
}
// if not current due to reset, set current now after clearing
if ( !current_ && was_reset_ ) {
was_reset_ = false;
current_ = true;
}
if ( footprint_clearing_enabled_ ) {
setConvexPolygonCost( transformed_footprint_, nav2_costmap_2d::FREE_SPACE );
}
switch ( combination_method_ ) {
case 0: // Overwrite
updateWithOverwrite( master_grid, min_i, min_j, max_i, max_j );
break;
case 1: // Maximum
updateWithMax( master_grid, min_i, min_j, max_i, max_j );
break;
default: // Nothing
break;
}
}
void
545 ObstacleLayer::addStaticObservation(
546 nav2_costmap_2d::Observation & obs,
547 bool marking, bool clearing )
{
if ( marking ) {
static_marking_observations_.push_back( obs );
}
if ( clearing ) {
static_clearing_observations_.push_back( obs );
}
}
void
558 ObstacleLayer::clearStaticObservations( bool marking, bool clearing )
{
if ( marking ) {
static_marking_observations_.clear( );
}
if ( clearing ) {
static_clearing_observations_.clear( );
}
}
bool
569 ObstacleLayer::getMarkingObservations( std::vector<Observation> & marking_observations ) const
{
bool current = true;
// get the marking observations
for ( unsigned int i = 0; i < marking_buffers_.size( ); ++i ) {
marking_buffers_[i]->lock( );
marking_buffers_[i]->getObservations( marking_observations );
current = marking_buffers_[i]->isCurrent( ) && current;
marking_buffers_[i]->unlock( );
}
marking_observations.insert(
marking_observations.end( ),
static_marking_observations_.begin( ), static_marking_observations_.end( ) );
return current;
}
bool
586 ObstacleLayer::getClearingObservations( std::vector<Observation> & clearing_observations ) const
{
bool current = true;
// get the clearing observations
for ( unsigned int i = 0; i < clearing_buffers_.size( ); ++i ) {
clearing_buffers_[i]->lock( );
clearing_buffers_[i]->getObservations( clearing_observations );
current = clearing_buffers_[i]->isCurrent( ) && current;
clearing_buffers_[i]->unlock( );
}
clearing_observations.insert(
clearing_observations.end( ),
static_clearing_observations_.begin( ), static_clearing_observations_.end( ) );
return current;
}
void
603 ObstacleLayer::raytraceFreespace(
604 const Observation & clearing_observation, double * min_x,
double * min_y,
double * max_x,
double * max_y )
{
double ox = clearing_observation.origin_.x;
double oy = clearing_observation.origin_.y;
const sensor_msgs::msg::PointCloud2 & cloud = *( clearing_observation.cloud_ );
// get the map coordinates of the origin of the sensor
unsigned int x0, y0;
if ( !worldToMap( ox, oy, x0, y0 ) ) {
RCLCPP_WARN(
logger_,
"Sensor origin at ( %.2f, %.2f ) is out of map bounds ( %.2f, %.2f ) to ( %.2f, %.2f ). "
"The costmap cannot raytrace for it.",
ox, oy,
origin_x_, origin_y_,
origin_x_ + getSizeInMetersX( ), origin_y_ + getSizeInMetersY( ) );
return;
}
// we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
double origin_x = origin_x_, origin_y = origin_y_;
double map_end_x = origin_x + size_x_ * resolution_;
double map_end_y = origin_y + size_y_ * resolution_;
touch( ox, oy, min_x, min_y, max_x, max_y );
// for each point in the cloud, we want to trace a line from the origin
// and clear obstacles along it
sensor_msgs::PointCloud2ConstIterator<float> iter_x( cloud, "x" );
sensor_msgs::PointCloud2ConstIterator<float> iter_y( cloud, "y" );
for ( ; iter_x != iter_x.end( ); ++iter_x, ++iter_y ) {
double wx = *iter_x;
double wy = *iter_y;
// now we also need to make sure that the enpoint we're raytracing
// to isn't off the costmap and scale if necessary
double a = wx - ox;
double b = wy - oy;
// the minimum value to raytrace from is the origin
if ( wx < origin_x ) {
double t = ( origin_x - ox ) / a;
wx = origin_x;
wy = oy + b * t;
}
if ( wy < origin_y ) {
double t = ( origin_y - oy ) / b;
wx = ox + a * t;
wy = origin_y;
}
// the maximum value to raytrace to is the end of the map
if ( wx > map_end_x ) {
double t = ( map_end_x - ox ) / a;
wx = map_end_x - .001;
wy = oy + b * t;
}
if ( wy > map_end_y ) {
double t = ( map_end_y - oy ) / b;
wx = ox + a * t;
wy = map_end_y - .001;
}
// now that the vector is scaled correctly... we'll get the map coordinates of its endpoint
unsigned int x1, y1;
// check for legality just in case
if ( !worldToMap( wx, wy, x1, y1 ) ) {
continue;
}
unsigned int cell_raytrace_max_range = cellDistance( clearing_observation.raytrace_max_range_ );
unsigned int cell_raytrace_min_range = cellDistance( clearing_observation.raytrace_min_range_ );
MarkCell marker( costmap_, FREE_SPACE );
// and finally... we can execute our trace to clear obstacles along that line
raytraceLine( marker, x0, y0, x1, y1, cell_raytrace_max_range, cell_raytrace_min_range );
updateRaytraceBounds(
ox, oy, wx, wy, clearing_observation.raytrace_max_range_,
clearing_observation.raytrace_min_range_, min_x, min_y, max_x,
max_y );
}
}
void
694 ObstacleLayer::activate( )
{
for ( auto & notifier : observation_notifiers_ ) {
notifier->clear( );
}
// if we're stopped we need to re-subscribe to topics
for ( unsigned int i = 0; i < observation_subscribers_.size( ); ++i ) {
if ( observation_subscribers_[i] != NULL ) {
observation_subscribers_[i]->subscribe( );
}
}
resetBuffersLastUpdated( );
}
void
710 ObstacleLayer::deactivate( )
{
for ( unsigned int i = 0; i < observation_subscribers_.size( ); ++i ) {
if ( observation_subscribers_[i] != NULL ) {
observation_subscribers_[i]->unsubscribe( );
}
}
}
void
720 ObstacleLayer::updateRaytraceBounds(
double ox, double oy, double wx, double wy, double max_range, double min_range,
double * min_x, double * min_y, double * max_x, double * max_y )
{
double dx = wx - ox, dy = wy - oy;
double full_distance = hypot( dx, dy );
if ( full_distance < min_range ) {
return;
}
double scale = std::min( 1.0, max_range / full_distance );
double ex = ox + dx * scale, ey = oy + dy * scale;
touch( ex, ey, min_x, min_y, max_x, max_y );
}
void
735 ObstacleLayer::reset( )
{
resetMaps( );
resetBuffersLastUpdated( );
current_ = false;
was_reset_ = true;
}
void
744 ObstacleLayer::resetBuffersLastUpdated( )
{
for ( unsigned int i = 0; i < observation_buffers_.size( ); ++i ) {
if ( observation_buffers_[i] ) {
observation_buffers_[i]->resetLastUpdated( );
}
}
}
} // namespace nav2_costmap_2d
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2018 David V. Lu!!
* Copyright ( c ) 2020, Bytes Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <angles/angles.h>
#include <algorithm>
#include <list>
#include <limits>
#include <string>
#include <vector>
#include "pluginlib/class_list_macros.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"
#include "nav2_costmap_2d/range_sensor_layer.hpp"
47 PLUGINLIB_EXPORT_CLASS( nav2_costmap_2d::RangeSensorLayer, nav2_costmap_2d::Layer )
using nav2_costmap_2d::LETHAL_OBSTACLE;
using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
using nav2_costmap_2d::NO_INFORMATION;
using namespace std::literals::chrono_literals;
namespace nav2_costmap_2d
{
58 RangeSensorLayer::RangeSensorLayer( ) {}
60 void RangeSensorLayer::onInitialize( )
{
current_ = true;
was_reset_ = false;
buffered_readings_ = 0;
last_reading_time_ = clock_->now( );
default_value_ = to_cost( 0.5 );
matchSize( );
resetRange( );
auto node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
declareParameter( "enabled", rclcpp::ParameterValue( true ) );
node->get_parameter( name_ + "." + "enabled", enabled_ );
declareParameter( "phi", rclcpp::ParameterValue( 1.2 ) );
node->get_parameter( name_ + "." + "phi", phi_v_ );
declareParameter( "inflate_cone", rclcpp::ParameterValue( 1.0 ) );
node->get_parameter( name_ + "." + "inflate_cone", inflate_cone_ );
declareParameter( "no_readings_timeout", rclcpp::ParameterValue( 0.0 ) );
node->get_parameter( name_ + "." + "no_readings_timeout", no_readings_timeout_ );
declareParameter( "clear_threshold", rclcpp::ParameterValue( 0.2 ) );
node->get_parameter( name_ + "." + "clear_threshold", clear_threshold_ );
declareParameter( "mark_threshold", rclcpp::ParameterValue( 0.8 ) );
node->get_parameter( name_ + "." + "mark_threshold", mark_threshold_ );
declareParameter( "clear_on_max_reading", rclcpp::ParameterValue( false ) );
node->get_parameter( name_ + "." + "clear_on_max_reading", clear_on_max_reading_ );
double temp_tf_tol = 0.0;
node->get_parameter( "transform_tolerance", temp_tf_tol );
transform_tolerance_ = tf2::durationFromSec( temp_tf_tol );
std::vector<std::string> topic_names{};
declareParameter( "topics", rclcpp::ParameterValue( topic_names ) );
node->get_parameter( name_ + "." + "topics", topic_names );
InputSensorType input_sensor_type = InputSensorType::ALL;
std::string sensor_type_name;
declareParameter( "input_sensor_type", rclcpp::ParameterValue( "ALL" ) );
node->get_parameter( name_ + "." + "input_sensor_type", sensor_type_name );
std::transform(
sensor_type_name.begin( ), sensor_type_name.end( ),
sensor_type_name.begin( ), ::toupper );
RCLCPP_INFO(
logger_, "%s: %s as input_sensor_type given",
name_.c_str( ), sensor_type_name.c_str( ) );
if ( sensor_type_name == "VARIABLE" ) {
input_sensor_type = InputSensorType::VARIABLE;
} else if ( sensor_type_name == "FIXED" ) {
input_sensor_type = InputSensorType::FIXED;
} else if ( sensor_type_name == "ALL" ) {
input_sensor_type = InputSensorType::ALL;
} else {
RCLCPP_ERROR(
logger_, "%s: Invalid input sensor type: %s. Defaulting to ALL.",
name_.c_str( ), sensor_type_name.c_str( ) );
}
// Validate topic names list: it must be a ( normally non-empty ) list of strings
if ( topic_names.empty( ) ) {
RCLCPP_FATAL(
logger_, "Invalid topic names list: it must"
"be a non-empty list of strings" );
return;
}
// Traverse the topic names list subscribing to all of them with the same callback method
for ( auto & topic_name : topic_names ) {
if ( input_sensor_type == InputSensorType::VARIABLE ) {
processRangeMessageFunc_ = std::bind(
&RangeSensorLayer::processVariableRangeMsg, this,
std::placeholders::_1 );
} else if ( input_sensor_type == InputSensorType::FIXED ) {
processRangeMessageFunc_ = std::bind(
&RangeSensorLayer::processFixedRangeMsg, this,
std::placeholders::_1 );
} else if ( input_sensor_type == InputSensorType::ALL ) {
processRangeMessageFunc_ = std::bind(
&RangeSensorLayer::processRangeMsg, this,
std::placeholders::_1 );
} else {
RCLCPP_ERROR(
logger_,
"%s: Invalid input sensor type: %s. Did you make a new type"
"and forgot to choose the subscriber for it?",
name_.c_str( ), sensor_type_name.c_str( ) );
}
range_subs_.push_back(
node->create_subscription<sensor_msgs::msg::Range>(
topic_name, rclcpp::SensorDataQoS( ), std::bind(
&RangeSensorLayer::bufferIncomingRangeMsg, this,
std::placeholders::_1 ) ) );
RCLCPP_INFO(
logger_, "RangeSensorLayer: subscribed to "
"topic %s", range_subs_.back( )->get_topic_name( ) );
}
global_frame_ = layered_costmap_->getGlobalFrameID( );
}
166 double RangeSensorLayer::gamma( double theta )
{
if ( fabs( theta ) > max_angle_ ) {
return 0.0;
} else {
return 1 - pow( theta / max_angle_, 2 );
}
}
175 double RangeSensorLayer::delta( double phi )
{
return 1 - ( 1 + tanh( 2 * ( phi - phi_v_ ) ) ) / 2;
}
180 void RangeSensorLayer::get_deltas( double angle, double * dx, double * dy )
{
double ta = tan( angle );
if ( ta == 0 ) {
*dx = 0;
} else {
*dx = resolution_ / ta;
}
*dx = copysign( *dx, cos( angle ) );
*dy = copysign( resolution_, sin( angle ) );
}
193 double RangeSensorLayer::sensor_model( double r, double phi, double theta )
{
double lbda = delta( phi ) * gamma( theta );
double delta = resolution_;
if ( phi >= 0.0 && phi < r - 2 * delta * r ) {
return ( 1 - lbda ) * ( 0.5 );
} else if ( phi < r - delta * r ) {
return lbda * 0.5 * pow( ( phi - ( r - 2 * delta * r ) ) / ( delta * r ), 2 ) +
( 1 - lbda ) * .5;
} else if ( phi < r + delta * r ) {
double J = ( r - phi ) / ( delta * r );
return lbda * ( ( 1 - ( 0.5 ) * pow( J, 2 ) ) - 0.5 ) + 0.5;
} else {
return 0.5;
}
}
212 void RangeSensorLayer::bufferIncomingRangeMsg(
213 const sensor_msgs::msg::Range::SharedPtr range_message )
{
range_message_mutex_.lock( );
range_msgs_buffer_.push_back( *range_message );
range_message_mutex_.unlock( );
}
220 void RangeSensorLayer::updateCostmap( )
{
std::list<sensor_msgs::msg::Range> range_msgs_buffer_copy;
range_message_mutex_.lock( );
range_msgs_buffer_copy = std::list<sensor_msgs::msg::Range>( range_msgs_buffer_ );
range_msgs_buffer_.clear( );
range_message_mutex_.unlock( );
for ( auto & range_msgs_it : range_msgs_buffer_copy ) {
processRangeMessageFunc_( range_msgs_it );
}
}
234 void RangeSensorLayer::processRangeMsg( sensor_msgs::msg::Range & range_message )
{
if ( range_message.min_range == range_message.max_range ) {
processFixedRangeMsg( range_message );
} else {
processVariableRangeMsg( range_message );
}
}
243 void RangeSensorLayer::processFixedRangeMsg( sensor_msgs::msg::Range & range_message )
{
if ( !std::isinf( range_message.range ) ) {
RCLCPP_ERROR(
logger_,
"Fixed distance ranger ( min_range == max_range ) in frame %s sent invalid value. "
"Only -Inf ( == object detected ) and Inf ( == no object detected ) are valid.",
range_message.header.frame_id.c_str( ) );
return;
}
bool clear_sensor_cone = false;
if ( range_message.range > 0 ) { // +inf
if ( !clear_on_max_reading_ ) {
return; // no clearing at all
}
clear_sensor_cone = true;
}
range_message.range = range_message.min_range;
updateCostmap( range_message, clear_sensor_cone );
}
268 void RangeSensorLayer::processVariableRangeMsg( sensor_msgs::msg::Range & range_message )
{
if ( range_message.range < range_message.min_range || range_message.range >
range_message.max_range )
{
return;
}
bool clear_sensor_cone = false;
if ( range_message.range >= range_message.max_range && clear_on_max_reading_ ) {
clear_sensor_cone = true;
}
updateCostmap( range_message, clear_sensor_cone );
}
285 void RangeSensorLayer::updateCostmap(
286 sensor_msgs::msg::Range & range_message,
287 bool clear_sensor_cone )
{
max_angle_ = range_message.field_of_view / 2;
geometry_msgs::msg::PointStamped in, out;
in.header.stamp = range_message.header.stamp;
in.header.frame_id = range_message.header.frame_id;
if ( !tf_->canTransform(
in.header.frame_id, global_frame_,
tf2_ros::fromMsg( in.header.stamp ),
tf2_ros::fromRclcpp( transform_tolerance_ ) ) )
{
RCLCPP_INFO(
logger_, "Range sensor layer can't transform from %s to %s",
global_frame_.c_str( ), in.header.frame_id.c_str( ) );
return;
}
tf_->transform( in, out, global_frame_, transform_tolerance_ );
double ox = out.point.x, oy = out.point.y;
in.point.x = range_message.range;
tf_->transform( in, out, global_frame_, transform_tolerance_ );
double tx = out.point.x, ty = out.point.y;
// calculate target props
double dx = tx - ox, dy = ty - oy, theta = atan2( dy, dx ), d = sqrt( dx * dx + dy * dy );
// Integer Bounds of Update
int bx0, by0, bx1, by1;
// Triangle that will be really updated; the other cells within bounds are ignored
// This triangle is formed by the origin and left and right sides of sonar cone
int Ox, Oy, Ax, Ay, Bx, By;
// Bounds includes the origin
worldToMapNoBounds( ox, oy, Ox, Oy );
bx1 = bx0 = Ox;
by1 = by0 = Oy;
touch( ox, oy, &min_x_, &min_y_, &max_x_, &max_y_ );
// Update Map with Target Point
unsigned int aa, ab;
if ( worldToMap( tx, ty, aa, ab ) ) {
setCost( aa, ab, 233 );
touch( tx, ty, &min_x_, &min_y_, &max_x_, &max_y_ );
}
double mx, my;
// Update left side of sonar cone
mx = ox + cos( theta - max_angle_ ) * d * 1.2;
my = oy + sin( theta - max_angle_ ) * d * 1.2;
worldToMapNoBounds( mx, my, Ax, Ay );
bx0 = std::min( bx0, Ax );
bx1 = std::max( bx1, Ax );
by0 = std::min( by0, Ay );
by1 = std::max( by1, Ay );
touch( mx, my, &min_x_, &min_y_, &max_x_, &max_y_ );
// Update right side of sonar cone
mx = ox + cos( theta + max_angle_ ) * d * 1.2;
my = oy + sin( theta + max_angle_ ) * d * 1.2;
worldToMapNoBounds( mx, my, Bx, By );
bx0 = std::min( bx0, Bx );
bx1 = std::max( bx1, Bx );
by0 = std::min( by0, By );
by1 = std::max( by1, By );
touch( mx, my, &min_x_, &min_y_, &max_x_, &max_y_ );
// Limit Bounds to Grid
bx0 = std::max( 0, bx0 );
by0 = std::max( 0, by0 );
bx1 = std::min( static_cast<int>( size_x_ ), bx1 );
by1 = std::min( static_cast<int>( size_y_ ), by1 );
for ( unsigned int x = bx0; x <= ( unsigned int )bx1; x++ ) {
for ( unsigned int y = by0; y <= ( unsigned int )by1; y++ ) {
bool update_xy_cell = true;
// Unless inflate_cone_ is set to 100 %, we update cells only within the
// ( partially inflated ) sensor cone, projected on the costmap as a triangle.
// 0 % corresponds to just the triangle, but if your sensor fov is very
// narrow, the covered area can become zero due to cell discretization.
// See wiki description for more details
if ( inflate_cone_ < 1.0 ) {
// Determine barycentric coordinates
int w0 = orient2d( Ax, Ay, Bx, By, x, y );
int w1 = orient2d( Bx, By, Ox, Oy, x, y );
int w2 = orient2d( Ox, Oy, Ax, Ay, x, y );
// Barycentric coordinates inside area threshold; this is not mathematically
// sound at all, but it works!
float bcciath = -static_cast<float>( inflate_cone_ ) * area( Ax, Ay, Bx, By, Ox, Oy );
update_xy_cell = w0 >= bcciath && w1 >= bcciath && w2 >= bcciath;
}
if ( update_xy_cell ) {
double wx, wy;
mapToWorld( x, y, wx, wy );
update_cell( ox, oy, theta, range_message.range, wx, wy, clear_sensor_cone );
}
}
}
buffered_readings_++;
last_reading_time_ = clock_->now( );
}
401 void RangeSensorLayer::update_cell(
double ox, double oy, double ot, double r,
403 double nx, double ny, bool clear )
{
unsigned int x, y;
if ( worldToMap( nx, ny, x, y ) ) {
double dx = nx - ox, dy = ny - oy;
double theta = atan2( dy, dx ) - ot;
theta = angles::normalize_angle( theta );
double phi = sqrt( dx * dx + dy * dy );
double sensor = 0.0;
if ( !clear ) {
sensor = sensor_model( r, phi, theta );
}
double prior = to_prob( getCost( x, y ) );
double prob_occ = sensor * prior;
double prob_not = ( 1 - sensor ) * ( 1 - prior );
double new_prob = prob_occ / ( prob_occ + prob_not );
RCLCPP_DEBUG(
logger_,
"%f %f | %f %f = %f", dx, dy, theta, phi, sensor );
RCLCPP_DEBUG(
logger_,
"%f | %f %f | %f", prior, prob_occ, prob_not, new_prob );
unsigned char c = to_cost( new_prob );
setCost( x, y, c );
}
}
431 void RangeSensorLayer::resetRange( )
{
min_x_ = min_y_ = std::numeric_limits<double>::max( );
max_x_ = max_y_ = -std::numeric_limits<double>::max( );
}
437 void RangeSensorLayer::updateBounds(
double robot_x, double robot_y,
double robot_yaw, double * min_x, double * min_y,
double * max_x, double * max_y )
{
robot_yaw = 0 + robot_yaw; // Avoid error if variable not in use
if ( layered_costmap_->isRolling( ) ) {
updateOrigin( robot_x - getSizeInMetersX( ) / 2, robot_y - getSizeInMetersY( ) / 2 );
}
updateCostmap( );
*min_x = std::min( *min_x, min_x_ );
*min_y = std::min( *min_y, min_y_ );
*max_x = std::max( *max_x, max_x_ );
*max_y = std::max( *max_y, max_y_ );
resetRange( );
if ( !enabled_ ) {
current_ = true;
return;
}
if ( buffered_readings_ == 0 ) {
if ( no_readings_timeout_ > 0.0 &&
( clock_->now( ) - last_reading_time_ ).seconds( ) >
no_readings_timeout_ )
{
RCLCPP_WARN(
logger_,
"No range readings received for %.2f seconds, while expected at least every %.2f seconds.",
( clock_->now( ) - last_reading_time_ ).seconds( ),
no_readings_timeout_ );
current_ = false;
}
}
}
476 void RangeSensorLayer::updateCosts(
477 nav2_costmap_2d::Costmap2D & master_grid,
int min_i, int min_j, int max_i, int max_j )
{
if ( !enabled_ ) {
return;
}
unsigned char * master_array = master_grid.getCharMap( );
unsigned int span = master_grid.getSizeInCellsX( );
unsigned char clear = to_cost( clear_threshold_ ), mark = to_cost( mark_threshold_ );
for ( int j = min_j; j < max_j; j++ ) {
unsigned int it = j * span + min_i;
for ( int i = min_i; i < max_i; i++ ) {
unsigned char prob = costmap_[it];
unsigned char current;
if ( prob == nav2_costmap_2d::NO_INFORMATION ) {
it++;
continue;
} else if ( prob > mark ) {
current = nav2_costmap_2d::LETHAL_OBSTACLE;
} else if ( prob < clear ) {
current = nav2_costmap_2d::FREE_SPACE;
} else {
it++;
continue;
}
unsigned char old_cost = master_array[it];
if ( old_cost == NO_INFORMATION || old_cost < current ) {
master_array[it] = current;
}
it++;
}
}
buffered_readings_ = 0;
// if not current due to reset, set current now after clearing
if ( !current_ && was_reset_ ) {
was_reset_ = false;
current_ = true;
}
}
523 void RangeSensorLayer::reset( )
{
RCLCPP_DEBUG( logger_, "Reseting range sensor layer..." );
deactivate( );
resetMaps( );
was_reset_ = true;
activate( );
}
532 void RangeSensorLayer::deactivate( )
{
range_msgs_buffer_.clear( );
}
537 void RangeSensorLayer::activate( )
{
range_msgs_buffer_.clear( );
}
} // namespace nav2_costmap_2d
1 /*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* Copyright ( c ) 2015, Fetch Robotics, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include "nav2_costmap_2d/static_layer.hpp"
#include <algorithm>
#include <string>
#include "pluginlib/class_list_macros.hpp"
#include "tf2/convert.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
49 PLUGINLIB_EXPORT_CLASS( nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer )
using nav2_costmap_2d::NO_INFORMATION;
using nav2_costmap_2d::LETHAL_OBSTACLE;
using nav2_costmap_2d::FREE_SPACE;
using rcl_interfaces::msg::ParameterType;
namespace nav2_costmap_2d
{
59 StaticLayer::StaticLayer( )
: map_buffer_( nullptr )
{
}
64 StaticLayer::~StaticLayer( )
{
}
void
69 StaticLayer::onInitialize( )
{
global_frame_ = layered_costmap_->getGlobalFrameID( );
getParameters( );
rclcpp::QoS map_qos( 10 ); // initialize to default
if ( map_subscribe_transient_local_ ) {
map_qos.transient_local( );
map_qos.reliable( );
map_qos.keep_last( 1 );
}
RCLCPP_INFO(
logger_,
"Subscribing to the map topic ( %s ) with %s durability",
map_topic_.c_str( ),
map_subscribe_transient_local_ ? "transient local" : "volatile" );
auto node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
map_sub_ = node->create_subscription<nav_msgs::msg::OccupancyGrid>(
map_topic_, map_qos,
std::bind( &StaticLayer::incomingMap, this, std::placeholders::_1 ) );
if ( subscribe_to_updates_ ) {
RCLCPP_INFO( logger_, "Subscribing to updates" );
map_update_sub_ = node->create_subscription<map_msgs::msg::OccupancyGridUpdate>(
map_topic_ + "_updates",
rclcpp::SystemDefaultsQoS( ),
std::bind( &StaticLayer::incomingUpdate, this, std::placeholders::_1 ) );
}
}
void
107 StaticLayer::activate( )
{
}
void
112 StaticLayer::deactivate( )
{
dyn_params_handler_.reset( );
}
void
118 StaticLayer::reset( )
{
has_updated_data_ = true;
current_ = false;
}
void
125 StaticLayer::getParameters( )
{
int temp_lethal_threshold = 0;
double temp_tf_tol = 0.0;
declareParameter( "enabled", rclcpp::ParameterValue( true ) );
declareParameter( "subscribe_to_updates", rclcpp::ParameterValue( false ) );
declareParameter( "map_subscribe_transient_local", rclcpp::ParameterValue( true ) );
declareParameter( "transform_tolerance", rclcpp::ParameterValue( 0.0 ) );
declareParameter( "map_topic", rclcpp::ParameterValue( "" ) );
auto node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
node->get_parameter( name_ + "." + "enabled", enabled_ );
node->get_parameter( name_ + "." + "subscribe_to_updates", subscribe_to_updates_ );
std::string private_map_topic, global_map_topic;
node->get_parameter( name_ + "." + "map_topic", private_map_topic );
node->get_parameter( "map_topic", global_map_topic );
if ( !private_map_topic.empty( ) ) {
map_topic_ = private_map_topic;
} else {
map_topic_ = global_map_topic;
}
node->get_parameter(
name_ + "." + "map_subscribe_transient_local",
map_subscribe_transient_local_ );
node->get_parameter( "track_unknown_space", track_unknown_space_ );
node->get_parameter( "use_maximum", use_maximum_ );
node->get_parameter( "lethal_cost_threshold", temp_lethal_threshold );
node->get_parameter( "unknown_cost_value", unknown_cost_value_ );
node->get_parameter( "trinary_costmap", trinary_costmap_ );
node->get_parameter( "transform_tolerance", temp_tf_tol );
// Enforce bounds
lethal_threshold_ = std::max( std::min( temp_lethal_threshold, 100 ), 0 );
map_received_ = false;
update_in_progress_.store( false );
transform_tolerance_ = tf2::durationFromSec( temp_tf_tol );
// Add callback for dynamic parameters
dyn_params_handler_ = node->add_on_set_parameters_callback(
std::bind(
&StaticLayer::dynamicParametersCallback,
this, std::placeholders::_1 ) );
}
void
176 StaticLayer::processMap( const nav_msgs::msg::OccupancyGrid & new_map )
{
RCLCPP_DEBUG( logger_, "StaticLayer: Process map" );
unsigned int size_x = new_map.info.width;
unsigned int size_y = new_map.info.height;
RCLCPP_DEBUG(
logger_,
"StaticLayer: Received a %d X %d map at %f m/pix", size_x, size_y,
new_map.info.resolution );
// resize costmap if size, resolution or origin do not match
Costmap2D * master = layered_costmap_->getCostmap( );
if ( !layered_costmap_->isRolling( ) && ( master->getSizeInCellsX( ) != size_x ||
master->getSizeInCellsY( ) != size_y ||
master->getResolution( ) != new_map.info.resolution ||
master->getOriginX( ) != new_map.info.origin.position.x ||
master->getOriginY( ) != new_map.info.origin.position.y ||
!layered_costmap_->isSizeLocked( ) ) )
{
// Update the size of the layered costmap ( and all layers, including this one )
RCLCPP_INFO(
logger_,
"StaticLayer: Resizing costmap to %d X %d at %f m/pix", size_x, size_y,
new_map.info.resolution );
layered_costmap_->resizeMap(
size_x, size_y, new_map.info.resolution,
new_map.info.origin.position.x,
new_map.info.origin.position.y,
true );
} else if ( size_x_ != size_x || size_y_ != size_y || // NOLINT
resolution_ != new_map.info.resolution ||
origin_x_ != new_map.info.origin.position.x ||
origin_y_ != new_map.info.origin.position.y )
{
// only update the size of the costmap stored locally in this layer
RCLCPP_INFO(
logger_,
"StaticLayer: Resizing static layer to %d X %d at %f m/pix", size_x, size_y,
new_map.info.resolution );
resizeMap(
size_x, size_y, new_map.info.resolution,
new_map.info.origin.position.x, new_map.info.origin.position.y );
}
unsigned int index = 0;
// we have a new map, update full size of map
std::lock_guard<Costmap2D::mutex_t> guard( *getMutex( ) );
// initialize the costmap with static data
for ( unsigned int i = 0; i < size_y; ++i ) {
for ( unsigned int j = 0; j < size_x; ++j ) {
unsigned char value = new_map.data[index];
costmap_[index] = interpretValue( value );
++index;
}
}
map_frame_ = new_map.header.frame_id;
x_ = y_ = 0;
width_ = size_x_;
height_ = size_y_;
has_updated_data_ = true;
current_ = true;
}
void
247 StaticLayer::matchSize( )
{
// If we are using rolling costmap, the static map size is
// unrelated to the size of the layered costmap
if ( !layered_costmap_->isRolling( ) ) {
Costmap2D * master = layered_costmap_->getCostmap( );
resizeMap(
master->getSizeInCellsX( ), master->getSizeInCellsY( ), master->getResolution( ),
master->getOriginX( ), master->getOriginY( ) );
}
}
unsigned char
260 StaticLayer::interpretValue( unsigned char value )
{
// check if the static value is above the unknown or lethal thresholds
if ( track_unknown_space_ && value == unknown_cost_value_ ) {
return NO_INFORMATION;
} else if ( !track_unknown_space_ && value == unknown_cost_value_ ) {
return FREE_SPACE;
} else if ( value >= lethal_threshold_ ) {
return LETHAL_OBSTACLE;
} else if ( trinary_costmap_ ) {
return FREE_SPACE;
}
double scale = static_cast<double>( value ) / lethal_threshold_;
return scale * LETHAL_OBSTACLE;
}
void
278 StaticLayer::incomingMap( const nav_msgs::msg::OccupancyGrid::SharedPtr new_map )
{
std::lock_guard<Costmap2D::mutex_t> guard( *getMutex( ) );
if ( !map_received_ ) {
map_received_ = true;
processMap( *new_map );
}
if ( update_in_progress_.load( ) ) {
map_buffer_ = new_map;
} else {
processMap( *new_map );
map_buffer_ = nullptr;
}
}
void
294 StaticLayer::incomingUpdate( map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr update )
{
std::lock_guard<Costmap2D::mutex_t> guard( *getMutex( ) );
if ( update->y < static_cast<int32_t>( y_ ) ||
y_ + height_ < update->y + update->height ||
update->x < static_cast<int32_t>( x_ ) ||
x_ + width_ < update->x + update->width )
{
RCLCPP_WARN(
logger_,
"StaticLayer: Map update ignored. Exceeds bounds of static layer.\n"
"Static layer origin: %d, %d bounds: %d X %d\n"
"Update origin: %d, %d bounds: %d X %d",
x_, y_, width_, height_, update->x, update->y, update->width,
update->height );
return;
}
if ( update->header.frame_id != map_frame_ ) {
RCLCPP_WARN(
logger_,
"StaticLayer: Map update ignored. Current map is in frame %s "
"but update was in frame %s",
map_frame_.c_str( ), update->header.frame_id.c_str( ) );
}
unsigned int di = 0;
for ( unsigned int y = 0; y < update->height; y++ ) {
unsigned int index_base = ( update->y + y ) * size_x_;
for ( unsigned int x = 0; x < update->width; x++ ) {
unsigned int index = index_base + x + update->x;
costmap_[index] = interpretValue( update->data[di++] );
}
}
has_updated_data_ = true;
}
void
334 StaticLayer::updateBounds(
double /*robot_x*/, double /*robot_y*/, double /*robot_yaw*/, double * min_x,
double * min_y,
double * max_x,
double * max_y )
{
if ( !map_received_ ) {
return;
}
std::lock_guard<Costmap2D::mutex_t> guard( *getMutex( ) );
update_in_progress_.store( true );
// If there is a new available map, load it.
if ( map_buffer_ ) {
processMap( *map_buffer_ );
map_buffer_ = nullptr;
}
if ( !layered_costmap_->isRolling( ) ) {
if ( !( has_updated_data_ || has_extra_bounds_ ) ) {
return;
}
}
useExtraBounds( min_x, min_y, max_x, max_y );
double wx, wy;
mapToWorld( x_, y_, wx, wy );
*min_x = std::min( wx, *min_x );
*min_y = std::min( wy, *min_y );
mapToWorld( x_ + width_, y_ + height_, wx, wy );
*max_x = std::max( wx, *max_x );
*max_y = std::max( wy, *max_y );
has_updated_data_ = false;
}
void
375 StaticLayer::updateCosts(
376 nav2_costmap_2d::Costmap2D & master_grid,
int min_i, int min_j, int max_i, int max_j )
{
std::lock_guard<Costmap2D::mutex_t> guard( *getMutex( ) );
if ( !enabled_ ) {
update_in_progress_.store( false );
return;
}
if ( !map_received_ ) {
static int count = 0;
// throttle warning down to only 1/10 message rate
if ( ++count == 10 ) {
RCLCPP_WARN( logger_, "Can't update static costmap layer, no map received" );
count = 0;
}
update_in_progress_.store( false );
return;
}
if ( !layered_costmap_->isRolling( ) ) {
// if not rolling, the layered costmap ( master_grid ) has same coordinates as this layer
if ( !use_maximum_ ) {
updateWithTrueOverwrite( master_grid, min_i, min_j, max_i, max_j );
} else {
updateWithMax( master_grid, min_i, min_j, max_i, max_j );
}
} else {
// If rolling window, the master_grid is unlikely to have same coordinates as this layer
unsigned int mx, my;
double wx, wy;
// Might even be in a different frame
geometry_msgs::msg::TransformStamped transform;
try {
transform = tf_->lookupTransform(
map_frame_, global_frame_, tf2::TimePointZero,
transform_tolerance_ );
} catch ( tf2::TransformException & ex ) {
RCLCPP_ERROR( logger_, "StaticLayer: %s", ex.what( ) );
update_in_progress_.store( false );
return;
}
// Copy map data given proper transformations
tf2::Transform tf2_transform;
tf2::fromMsg( transform.transform, tf2_transform );
for ( int i = min_i; i < max_i; ++i ) {
for ( int j = min_j; j < max_j; ++j ) {
// Convert master_grid coordinates ( i, j ) into global_frame_( wx, wy ) coordinates
layered_costmap_->getCostmap( )->mapToWorld( i, j, wx, wy );
// Transform from global_frame_ to map_frame_
tf2::Vector3 p( wx, wy, 0 );
p = tf2_transform * p;
// Set master_grid with cell from map
if ( worldToMap( p.x( ), p.y( ), mx, my ) ) {
if ( !use_maximum_ ) {
master_grid.setCost( i, j, getCost( mx, my ) );
} else {
master_grid.setCost( i, j, std::max( getCost( mx, my ), master_grid.getCost( i, j ) ) );
}
}
}
}
}
update_in_progress_.store( false );
current_ = true;
}
/**
* @brief Callback executed when a parameter change is detected
* @param event ParameterEvent message
*/
rcl_interfaces::msg::SetParametersResult
448 StaticLayer::dynamicParametersCallback(
449 std::vector<rclcpp::Parameter> parameters )
{
std::lock_guard<Costmap2D::mutex_t> guard( *getMutex( ) );
rcl_interfaces::msg::SetParametersResult result;
for ( auto parameter : parameters ) {
const auto & param_type = parameter.get_type( );
const auto & param_name = parameter.get_name( );
if ( param_name == name_ + "." + "map_subscribe_transient_local" ||
param_name == name_ + "." + "map_topic" ||
param_name == name_ + "." + "subscribe_to_updates" )
{
RCLCPP_WARN(
logger_, "%s is not a dynamic parameter "
"cannot be changed while running. Rejecting parameter update.", param_name.c_str( ) );
} else if ( param_type == ParameterType::PARAMETER_DOUBLE ) {
if ( param_name == name_ + "." + "transform_tolerance" ) {
transform_tolerance_ = tf2::durationFromSec( parameter.as_double( ) );
}
} else if ( param_type == ParameterType::PARAMETER_BOOL ) {
if ( param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool( ) ) {
enabled_ = parameter.as_bool( );
x_ = y_ = 0;
width_ = size_x_;
height_ = size_y_;
has_updated_data_ = true;
current_ = false;
}
}
}
result.successful = true;
return result;
}
} // namespace nav2_costmap_2d
1 /*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include "nav2_costmap_2d/voxel_layer.hpp"
#include <algorithm>
#include <cassert>
#include <vector>
#include <memory>
#include <utility>
#include "pluginlib/class_list_macros.hpp"
#include "sensor_msgs/point_cloud2_iterator.hpp"
#define VOXEL_BITS 16
51 PLUGINLIB_EXPORT_CLASS( nav2_costmap_2d::VoxelLayer, nav2_costmap_2d::Layer )
using nav2_costmap_2d::NO_INFORMATION;
using nav2_costmap_2d::LETHAL_OBSTACLE;
using nav2_costmap_2d::FREE_SPACE;
using rcl_interfaces::msg::ParameterType;
namespace nav2_costmap_2d
{
61 void VoxelLayer::onInitialize( )
{
ObstacleLayer::onInitialize( );
declareParameter( "enabled", rclcpp::ParameterValue( true ) );
declareParameter( "footprint_clearing_enabled", rclcpp::ParameterValue( true ) );
declareParameter( "max_obstacle_height", rclcpp::ParameterValue( 2.0 ) );
declareParameter( "z_voxels", rclcpp::ParameterValue( 10 ) );
declareParameter( "origin_z", rclcpp::ParameterValue( 0.0 ) );
declareParameter( "z_resolution", rclcpp::ParameterValue( 0.2 ) );
declareParameter( "unknown_threshold", rclcpp::ParameterValue( 15 ) );
declareParameter( "mark_threshold", rclcpp::ParameterValue( 0 ) );
declareParameter( "combination_method", rclcpp::ParameterValue( 1 ) );
declareParameter( "publish_voxel_map", rclcpp::ParameterValue( false ) );
auto node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
node->get_parameter( name_ + "." + "enabled", enabled_ );
node->get_parameter( name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_ );
node->get_parameter( name_ + "." + "max_obstacle_height", max_obstacle_height_ );
node->get_parameter( name_ + "." + "z_voxels", size_z_ );
node->get_parameter( name_ + "." + "origin_z", origin_z_ );
node->get_parameter( name_ + "." + "z_resolution", z_resolution_ );
node->get_parameter( name_ + "." + "unknown_threshold", unknown_threshold_ );
node->get_parameter( name_ + "." + "mark_threshold", mark_threshold_ );
node->get_parameter( name_ + "." + "combination_method", combination_method_ );
node->get_parameter( name_ + "." + "publish_voxel_map", publish_voxel_ );
auto custom_qos = rclcpp::QoS( rclcpp::KeepLast( 1 ) ).transient_local( ).reliable( );
if ( publish_voxel_ ) {
voxel_pub_ = node->create_publisher<nav2_msgs::msg::VoxelGrid>(
"voxel_grid", custom_qos );
voxel_pub_->on_activate( );
}
clearing_endpoints_pub_ = node->create_publisher<sensor_msgs::msg::PointCloud2>(
"clearing_endpoints", custom_qos );
clearing_endpoints_pub_->on_activate( );
unknown_threshold_ += ( VOXEL_BITS - size_z_ );
matchSize( );
// Add callback for dynamic parameters
dyn_params_handler_ = node->add_on_set_parameters_callback(
std::bind(
&VoxelLayer::dynamicParametersCallback,
this, std::placeholders::_1 ) );
}
114 VoxelLayer::~VoxelLayer( )
{
dyn_params_handler_.reset( );
}
119 void VoxelLayer::matchSize( )
{
std::lock_guard<Costmap2D::mutex_t> guard( *getMutex( ) );
ObstacleLayer::matchSize( );
voxel_grid_.resize( size_x_, size_y_, size_z_ );
assert( voxel_grid_.sizeX( ) == size_x_ && voxel_grid_.sizeY( ) == size_y_ );
}
127 void VoxelLayer::reset( )
{
// Call the base class method before adding our own functionality
ObstacleLayer::reset( );
resetMaps( );
}
134 void VoxelLayer::resetMaps( )
{
// Call the base class method before adding our own functionality
// Note: at the time this was written, ObstacleLayer doesn't implement
// resetMaps so this goes to the next layer down Costmap2DLayer which also
// doesn't implement this, so it actually goes all the way to Costmap2D
ObstacleLayer::resetMaps( );
voxel_grid_.reset( );
}
144 void VoxelLayer::updateBounds(
double robot_x, double robot_y, double robot_yaw, double * min_x,
double * min_y, double * max_x, double * max_y )
{
std::lock_guard<Costmap2D::mutex_t> guard( *getMutex( ) );
if ( rolling_window_ ) {
updateOrigin( robot_x - getSizeInMetersX( ) / 2, robot_y - getSizeInMetersY( ) / 2 );
}
if ( !enabled_ ) {
return;
}
useExtraBounds( min_x, min_y, max_x, max_y );
bool current = true;
std::vector<Observation> observations, clearing_observations;
// get the marking observations
current = getMarkingObservations( observations ) && current;
// get the clearing observations
current = getClearingObservations( clearing_observations ) && current;
// update the global current status
current_ = current;
// raytrace freespace
for ( unsigned int i = 0; i < clearing_observations.size( ); ++i ) {
raytraceFreespace( clearing_observations[i], min_x, min_y, max_x, max_y );
}
// place the new obstacles into a priority queue... each with a priority of zero to begin with
for ( std::vector<Observation>::const_iterator it = observations.begin( ); it != observations.end( );
++it )
{
const Observation & obs = *it;
const sensor_msgs::msg::PointCloud2 & cloud = *( obs.cloud_ );
double sq_obstacle_max_range = obs.obstacle_max_range_ * obs.obstacle_max_range_;
double sq_obstacle_min_range = obs.obstacle_min_range_ * obs.obstacle_min_range_;
sensor_msgs::PointCloud2ConstIterator<float> iter_x( cloud, "x" );
sensor_msgs::PointCloud2ConstIterator<float> iter_y( cloud, "y" );
sensor_msgs::PointCloud2ConstIterator<float> iter_z( cloud, "z" );
for ( ; iter_x != iter_x.end( ); ++iter_x, ++iter_y, ++iter_z ) {
// if the obstacle is too high or too far away from the robot we won't add it
if ( *iter_z > max_obstacle_height_ ) {
continue;
}
// compute the squared distance from the hitpoint to the pointcloud's origin
double sq_dist = ( *iter_x - obs.origin_.x ) * ( *iter_x - obs.origin_.x ) +
( *iter_y - obs.origin_.y ) * ( *iter_y - obs.origin_.y ) +
( *iter_z - obs.origin_.z ) * ( *iter_z - obs.origin_.z );
// if the point is far enough away... we won't consider it
if ( sq_dist >= sq_obstacle_max_range ) {
continue;
}
// If the point is too close, do not consider it
if ( sq_dist < sq_obstacle_min_range ) {
continue;
}
// now we need to compute the map coordinates for the observation
unsigned int mx, my, mz;
if ( *iter_z < origin_z_ ) {
if ( !worldToMap3D( *iter_x, *iter_y, origin_z_, mx, my, mz ) ) {
continue;
}
} else if ( !worldToMap3D( *iter_x, *iter_y, *iter_z, mx, my, mz ) ) {
continue;
}
// mark the cell in the voxel grid and check if we should also mark it in the costmap
if ( voxel_grid_.markVoxelInMap( mx, my, mz, mark_threshold_ ) ) {
unsigned int index = getIndex( mx, my );
costmap_[index] = LETHAL_OBSTACLE;
touch(
static_cast<double>( *iter_x ), static_cast<double>( *iter_y ),
min_x, min_y, max_x, max_y );
}
}
}
if ( publish_voxel_ ) {
auto grid_msg = std::make_unique<nav2_msgs::msg::VoxelGrid>( );
unsigned int size = voxel_grid_.sizeX( ) * voxel_grid_.sizeY( );
grid_msg->size_x = voxel_grid_.sizeX( );
grid_msg->size_y = voxel_grid_.sizeY( );
grid_msg->size_z = voxel_grid_.sizeZ( );
grid_msg->data.resize( size );
memcpy( &grid_msg->data[0], voxel_grid_.getData( ), size * sizeof( unsigned int ) );
grid_msg->origin.x = origin_x_;
grid_msg->origin.y = origin_y_;
grid_msg->origin.z = origin_z_;
grid_msg->resolutions.x = resolution_;
grid_msg->resolutions.y = resolution_;
grid_msg->resolutions.z = z_resolution_;
grid_msg->header.frame_id = global_frame_;
grid_msg->header.stamp = clock_->now( );
voxel_pub_->publish( std::move( grid_msg ) );
}
updateFootprint( robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y );
}
258 void VoxelLayer::raytraceFreespace(
259 const Observation & clearing_observation, double * min_x,
double * min_y,
double * max_x,
double * max_y )
{
auto clearing_endpoints_ = std::make_unique<sensor_msgs::msg::PointCloud2>( );
if ( clearing_observation.cloud_->height == 0 || clearing_observation.cloud_->width == 0 ) {
return;
}
double sensor_x, sensor_y, sensor_z;
double ox = clearing_observation.origin_.x;
double oy = clearing_observation.origin_.y;
double oz = clearing_observation.origin_.z;
if ( !worldToMap3DFloat( ox, oy, oz, sensor_x, sensor_y, sensor_z ) ) {
RCLCPP_WARN(
logger_,
"Sensor origin at ( %.2f, %.2f %.2f ) is out of map bounds ( %.2f, %.2f, %.2f ) to ( %.2f, %.2f, %.2f ). "
"The costmap cannot raytrace for it.",
ox, oy, oz,
ox + getSizeInMetersX( ), oy + getSizeInMetersY( ), oz + getSizeInMetersZ( ),
origin_x_, origin_y_, origin_z_ );
return;
}
bool publish_clearing_points;
{
auto node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
publish_clearing_points = ( node->count_subscribers( "clearing_endpoints" ) > 0 );
}
clearing_endpoints_->data.clear( );
clearing_endpoints_->width = clearing_observation.cloud_->width;
clearing_endpoints_->height = clearing_observation.cloud_->height;
clearing_endpoints_->is_dense = true;
clearing_endpoints_->is_bigendian = false;
sensor_msgs::PointCloud2Modifier modifier( *clearing_endpoints_ );
modifier.setPointCloud2Fields(
3, "x", 1, sensor_msgs::msg::PointField::FLOAT32,
"y", 1, sensor_msgs::msg::PointField::FLOAT32,
"z", 1, sensor_msgs::msg::PointField::FLOAT32 );
sensor_msgs::PointCloud2Iterator<float> clearing_endpoints_iter_x( *clearing_endpoints_, "x" );
sensor_msgs::PointCloud2Iterator<float> clearing_endpoints_iter_y( *clearing_endpoints_, "y" );
sensor_msgs::PointCloud2Iterator<float> clearing_endpoints_iter_z( *clearing_endpoints_, "z" );
// we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
double map_end_x = origin_x_ + getSizeInMetersX( );
double map_end_y = origin_y_ + getSizeInMetersY( );
double map_end_z = origin_z_ + getSizeInMetersZ( );
sensor_msgs::PointCloud2ConstIterator<float> iter_x( *( clearing_observation.cloud_ ), "x" );
sensor_msgs::PointCloud2ConstIterator<float> iter_y( *( clearing_observation.cloud_ ), "y" );
sensor_msgs::PointCloud2ConstIterator<float> iter_z( *( clearing_observation.cloud_ ), "z" );
for ( ; iter_x != iter_x.end( ); ++iter_x, ++iter_y, ++iter_z ) {
double wpx = *iter_x;
double wpy = *iter_y;
double wpz = *iter_z;
double distance = dist( ox, oy, oz, wpx, wpy, wpz );
double scaling_fact = 1.0;
scaling_fact = std::max( std::min( scaling_fact, ( distance - 2 * resolution_ ) / distance ), 0.0 );
wpx = scaling_fact * ( wpx - ox ) + ox;
wpy = scaling_fact * ( wpy - oy ) + oy;
wpz = scaling_fact * ( wpz - oz ) + oz;
double a = wpx - ox;
double b = wpy - oy;
double c = wpz - oz;
double t = 1.0;
// we can only raytrace to a maximum z height
if ( wpz > map_end_z ) {
// we know we want the vector's z value to be max_z
t = std::max( 0.0, std::min( t, ( map_end_z - 0.01 - oz ) / c ) );
} else if ( wpz < origin_z_ ) {
// and we can only raytrace down to the floor
// we know we want the vector's z value to be 0.0
t = std::min( t, ( origin_z_ - oz ) / c );
}
// the minimum value to raytrace from is the origin
if ( wpx < origin_x_ ) {
t = std::min( t, ( origin_x_ - ox ) / a );
}
if ( wpy < origin_y_ ) {
t = std::min( t, ( origin_y_ - oy ) / b );
}
// the maximum value to raytrace to is the end of the map
if ( wpx > map_end_x ) {
t = std::min( t, ( map_end_x - ox ) / a );
}
if ( wpy > map_end_y ) {
t = std::min( t, ( map_end_y - oy ) / b );
}
wpx = ox + a * t;
wpy = oy + b * t;
wpz = oz + c * t;
double point_x, point_y, point_z;
if ( worldToMap3DFloat( wpx, wpy, wpz, point_x, point_y, point_z ) ) {
unsigned int cell_raytrace_max_range = cellDistance( clearing_observation.raytrace_max_range_ );
unsigned int cell_raytrace_min_range = cellDistance( clearing_observation.raytrace_min_range_ );
// voxel_grid_.markVoxelLine( sensor_x, sensor_y, sensor_z, point_x, point_y, point_z );
voxel_grid_.clearVoxelLineInMap(
sensor_x, sensor_y, sensor_z, point_x, point_y, point_z,
costmap_,
unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,
cell_raytrace_max_range, cell_raytrace_min_range );
updateRaytraceBounds(
ox, oy, wpx, wpy, clearing_observation.raytrace_max_range_,
clearing_observation.raytrace_min_range_, min_x, min_y,
max_x,
max_y );
if ( publish_clearing_points ) {
*clearing_endpoints_iter_x = wpx;
*clearing_endpoints_iter_y = wpy;
*clearing_endpoints_iter_z = wpz;
++clearing_endpoints_iter_x;
++clearing_endpoints_iter_y;
++clearing_endpoints_iter_z;
}
}
}
if ( publish_clearing_points ) {
clearing_endpoints_->header.frame_id = global_frame_;
clearing_endpoints_->header.stamp = clearing_observation.cloud_->header.stamp;
clearing_endpoints_pub_->publish( std::move( clearing_endpoints_ ) );
}
}
408 void VoxelLayer::updateOrigin( double new_origin_x, double new_origin_y )
{
// project the new origin into the grid
int cell_ox, cell_oy;
cell_ox = static_cast<int>( ( new_origin_x - origin_x_ ) / resolution_ );
cell_oy = static_cast<int>( ( new_origin_y - origin_y_ ) / resolution_ );
// compute the associated world coordinates for the origin cell
// beacuase we want to keep things grid-aligned
double new_grid_ox, new_grid_oy;
new_grid_ox = origin_x_ + cell_ox * resolution_;
new_grid_oy = origin_y_ + cell_oy * resolution_;
// To save casting from unsigned int to int a bunch of times
int size_x = size_x_;
int size_y = size_y_;
// we need to compute the overlap of the new and existing windows
int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
lower_left_x = std::min( std::max( cell_ox, 0 ), size_x );
lower_left_y = std::min( std::max( cell_oy, 0 ), size_y );
upper_right_x = std::min( std::max( cell_ox + size_x, 0 ), size_x );
upper_right_y = std::min( std::max( cell_oy + size_y, 0 ), size_y );
unsigned int cell_size_x = upper_right_x - lower_left_x;
unsigned int cell_size_y = upper_right_y - lower_left_y;
// we need a map to store the obstacles in the window temporarily
unsigned char * local_map = new unsigned char[cell_size_x * cell_size_y];
unsigned int * local_voxel_map = new unsigned int[cell_size_x * cell_size_y];
unsigned int * voxel_map = voxel_grid_.getData( );
// copy the local window in the costmap to the local map
copyMapRegion(
costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x,
cell_size_x,
cell_size_y );
copyMapRegion(
voxel_map, lower_left_x, lower_left_y, size_x_, local_voxel_map, 0, 0, cell_size_x,
cell_size_x,
cell_size_y );
// we'll reset our maps to unknown space if appropriate
resetMaps( );
// update the origin with the appropriate world coordinates
origin_x_ = new_grid_ox;
origin_y_ = new_grid_oy;
// compute the starting cell location for copying data back in
int start_x = lower_left_x - cell_ox;
int start_y = lower_left_y - cell_oy;
// now we want to copy the overlapping information back into the map, but in its new location
copyMapRegion(
local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x,
cell_size_y );
copyMapRegion(
local_voxel_map, 0, 0, cell_size_x, voxel_map, start_x, start_y, size_x_,
cell_size_x,
cell_size_y );
// make sure to clean up
delete[] local_map;
delete[] local_voxel_map;
}
/**
* @brief Callback executed when a parameter change is detected
* @param event ParameterEvent message
*/
rcl_interfaces::msg::SetParametersResult
480 VoxelLayer::dynamicParametersCallback(
481 std::vector<rclcpp::Parameter> parameters )
{
std::lock_guard<Costmap2D::mutex_t> guard( *getMutex( ) );
rcl_interfaces::msg::SetParametersResult result;
bool resize_map_needed = false;
for ( auto parameter : parameters ) {
const auto & param_type = parameter.get_type( );
const auto & param_name = parameter.get_name( );
if ( param_type == ParameterType::PARAMETER_DOUBLE ) {
if ( param_name == name_ + "." + "max_obstacle_height" ) {
max_obstacle_height_ = parameter.as_double( );
} else if ( param_name == name_ + "." + "origin_z" ) {
origin_z_ = parameter.as_double( );
resize_map_needed = true;
} else if ( param_name == name_ + "." + "z_resolution" ) {
z_resolution_ = parameter.as_double( );
resize_map_needed = true;
}
} else if ( param_type == ParameterType::PARAMETER_BOOL ) {
if ( param_name == name_ + "." + "enabled" ) {
enabled_ = parameter.as_bool( );
current_ = false;
} else if ( param_name == name_ + "." + "footprint_clearing_enabled" ) {
footprint_clearing_enabled_ = parameter.as_bool( );
} else if ( param_name == name_ + "." + "publish_voxel_map" ) {
RCLCPP_WARN(
logger_, "publish voxel map is not a dynamic parameter "
"cannot be changed while running. Rejecting parameter update." );
continue;
}
} else if ( param_type == ParameterType::PARAMETER_INTEGER ) {
if ( param_name == name_ + "." + "z_voxels" ) {
size_z_ = parameter.as_int( );
resize_map_needed = true;
} else if ( param_name == name_ + "." + "unknown_threshold" ) {
unknown_threshold_ = parameter.as_int( ) + ( VOXEL_BITS - size_z_ );
} else if ( param_name == name_ + "." + "mark_threshold" ) {
mark_threshold_ = parameter.as_int( );
} else if ( param_name == name_ + "." + "combination_method" ) {
combination_method_ = parameter.as_int( );
}
}
}
if ( resize_map_needed ) {
matchSize( );
}
result.successful = true;
return result;
}
} // namespace nav2_costmap_2d
1 /*
* Copyright ( c ) 2012, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES ( INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE )
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* author: Dave Hershberger
*/
#include <cstdio> // for EOF
#include <string>
#include <sstream>
#include <vector>
namespace nav2_costmap_2d
{
/** @brief Parse a vector of vector of floats from a string.
* @param input
* @param error_return
* Syntax is [[1.0, 2.0], [3.3, 4.4, 5.5], ...] */
44 std::vector<std::vector<float>> parseVVF( const std::string & input, std::string & error_return )
{
std::vector<std::vector<float>> result;
std::stringstream input_ss( input );
int depth = 0;
std::vector<float> current_vector;
while ( !!input_ss && !input_ss.eof( ) ) {
switch ( input_ss.peek( ) ) {
case EOF:
break;
case '[':
depth++;
if ( depth > 2 ) {
error_return = "Array depth greater than 2";
return result;
}
input_ss.get( );
current_vector.clear( );
break;
case ']':
depth--;
if ( depth < 0 ) {
error_return = "More close ] than open [";
return result;
}
input_ss.get( );
if ( depth == 1 ) {
result.push_back( current_vector );
}
break;
case ', ':
case ' ':
case '\t':
input_ss.get( );
break;
default: // All other characters should be part of the numbers.
if ( depth != 2 ) {
std::stringstream err_ss;
err_ss << "Numbers at depth other than 2. Char was '" << char( input_ss.peek( ) ) << "'.";
error_return = err_ss.str( );
return result;
}
float value;
input_ss >> value;
if ( !!input_ss ) {
current_vector.push_back( value );
}
break;
}
}
if ( depth != 0 ) {
error_return = "Unterminated vector string.";
} else {
error_return = "";
}
return result;
}
} // end namespace nav2_costmap_2d
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <vector>
#include <string>
#include <algorithm>
#include <memory>
#include "nav2_costmap_2d/clear_costmap_service.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
namespace nav2_costmap_2d
{
using std::vector;
using std::string;
using std::shared_ptr;
using std::any_of;
using ClearExceptRegion = nav2_msgs::srv::ClearCostmapExceptRegion;
using ClearAroundRobot = nav2_msgs::srv::ClearCostmapAroundRobot;
using ClearEntirely = nav2_msgs::srv::ClearEntireCostmap;
34 ClearCostmapService::ClearCostmapService(
35 const nav2_util::LifecycleNode::WeakPtr & parent,
36 Costmap2DROS & costmap )
: costmap_( costmap )
{
auto node = parent.lock( );
logger_ = node->get_logger( );
reset_value_ = costmap_.getCostmap( )->getDefaultValue( );
node->get_parameter( "clearable_layers", clearable_layers_ );
clear_except_service_ = node->create_service<ClearExceptRegion>(
"clear_except_" + costmap_.getName( ),
std::bind(
&ClearCostmapService::clearExceptRegionCallback, this,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3 ) );
clear_around_service_ = node->create_service<ClearAroundRobot>(
"clear_around_" + costmap.getName( ),
std::bind(
&ClearCostmapService::clearAroundRobotCallback, this,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3 ) );
clear_entire_service_ = node->create_service<ClearEntirely>(
"clear_entirely_" + costmap_.getName( ),
std::bind(
&ClearCostmapService::clearEntireCallback, this,
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3 ) );
}
64 void ClearCostmapService::clearExceptRegionCallback(
65 const shared_ptr<rmw_request_id_t>/*request_header*/,
66 const shared_ptr<ClearExceptRegion::Request> request,
67 const shared_ptr<ClearExceptRegion::Response>/*response*/ )
{
RCLCPP_INFO(
logger_,
( "Received request to clear except a region the " + costmap_.getName( ) ).c_str( ) );
clearRegion( request->reset_distance, true );
}
76 void ClearCostmapService::clearAroundRobotCallback(
77 const shared_ptr<rmw_request_id_t>/*request_header*/,
78 const shared_ptr<ClearAroundRobot::Request> request,
79 const shared_ptr<ClearAroundRobot::Response>/*response*/ )
{
clearRegion( request->reset_distance, false );
}
84 void ClearCostmapService::clearEntireCallback(
85 const std::shared_ptr<rmw_request_id_t>/*request_header*/,
86 const std::shared_ptr<ClearEntirely::Request>/*request*/,
87 const std::shared_ptr<ClearEntirely::Response>/*response*/ )
{
RCLCPP_INFO(
logger_,
( "Received request to clear entirely the " + costmap_.getName( ) ).c_str( ) );
clearEntirely( );
}
96 void ClearCostmapService::clearRegion( const double reset_distance, bool invert )
{
double x, y;
if ( !getPosition( x, y ) ) {
RCLCPP_ERROR(
logger_,
"Cannot clear map because robot pose cannot be retrieved." );
return;
}
auto layers = costmap_.getLayeredCostmap( )->getPlugins( );
for ( auto & layer : *layers ) {
if ( layer->isClearable( ) ) {
auto costmap_layer = std::static_pointer_cast<CostmapLayer>( layer );
clearLayerRegion( costmap_layer, x, y, reset_distance, invert );
}
}
// AlexeyMerzlyakov: No need to clear layer region for costmap filters
// as they are always supposed to be not clearable.
}
120 void ClearCostmapService::clearLayerRegion(
121 shared_ptr<CostmapLayer> & costmap, double pose_x, double pose_y, double reset_distance,
122 bool invert )
{
std::unique_lock<Costmap2D::mutex_t> lock( *( costmap->getMutex( ) ) );
double start_point_x = pose_x - reset_distance / 2;
double start_point_y = pose_y - reset_distance / 2;
double end_point_x = start_point_x + reset_distance;
double end_point_y = start_point_y + reset_distance;
int start_x, start_y, end_x, end_y;
costmap->worldToMapEnforceBounds( start_point_x, start_point_y, start_x, start_y );
costmap->worldToMapEnforceBounds( end_point_x, end_point_y, end_x, end_y );
costmap->clearArea( start_x, start_y, end_x, end_y, invert );
double ox = costmap->getOriginX( ), oy = costmap->getOriginY( );
double width = costmap->getSizeInMetersX( ), height = costmap->getSizeInMetersY( );
costmap->addExtraBounds( ox, oy, ox + width, oy + height );
}
142 void ClearCostmapService::clearEntirely( )
{
std::unique_lock<Costmap2D::mutex_t> lock( *( costmap_.getCostmap( )->getMutex( ) ) );
costmap_.resetLayers( );
}
148 bool ClearCostmapService::getPosition( double & x, double & y ) const
{
geometry_msgs::msg::PoseStamped pose;
if ( !costmap_.getRobotPose( pose ) ) {
return false;
}
x = pose.pose.position.x;
y = pose.pose.position.y;
return true;
}
} // namespace nav2_costmap_2d
1 /*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include "nav2_costmap_2d/costmap_2d.hpp"
#include <algorithm>
#include <cstdio>
#include <string>
#include <vector>
#include "nav2_costmap_2d/cost_values.hpp"
#include "nav2_util/occ_grid_values.hpp"
namespace nav2_costmap_2d
{
49 Costmap2D::Costmap2D(
unsigned int cells_size_x, unsigned int cells_size_y, double resolution,
double origin_x, double origin_y, unsigned char default_value )
: size_x_( cells_size_x ), size_y_( cells_size_y ), resolution_( resolution ), origin_x_( origin_x ),
origin_y_( origin_y ), costmap_( NULL ), default_value_( default_value )
{
access_ = new mutex_t( );
// create the costmap
initMaps( size_x_, size_y_ );
resetMaps( );
}
62 Costmap2D::Costmap2D( const nav_msgs::msg::OccupancyGrid & map )
: default_value_( FREE_SPACE )
{
access_ = new mutex_t( );
// fill local variables
size_x_ = map.info.width;
size_y_ = map.info.height;
resolution_ = map.info.resolution;
origin_x_ = map.info.origin.position.x;
origin_y_ = map.info.origin.position.y;
// create the costmap
costmap_ = new unsigned char[size_x_ * size_y_];
// fill the costmap with a data
int8_t data;
for ( unsigned int it = 0; it < size_x_ * size_y_; it++ ) {
data = map.data[it];
if ( data == nav2_util::OCC_GRID_UNKNOWN ) {
costmap_[it] = NO_INFORMATION;
} else {
// Linear conversion from OccupancyGrid data range [OCC_GRID_FREE..OCC_GRID_OCCUPIED]
// to costmap data range [FREE_SPACE..LETHAL_OBSTACLE]
costmap_[it] = std::round(
static_cast<double>( data ) * ( LETHAL_OBSTACLE - FREE_SPACE ) /
( nav2_util::OCC_GRID_OCCUPIED - nav2_util::OCC_GRID_FREE ) );
}
}
}
93 void Costmap2D::deleteMaps( )
{
// clean up data
std::unique_lock<mutex_t> lock( *access_ );
delete[] costmap_;
costmap_ = NULL;
}
101 void Costmap2D::initMaps( unsigned int size_x, unsigned int size_y )
{
std::unique_lock<mutex_t> lock( *access_ );
delete[] costmap_;
costmap_ = new unsigned char[size_x * size_y];
}
108 void Costmap2D::resizeMap(
unsigned int size_x, unsigned int size_y, double resolution,
double origin_x, double origin_y )
{
size_x_ = size_x;
size_y_ = size_y;
resolution_ = resolution;
origin_x_ = origin_x;
origin_y_ = origin_y;
initMaps( size_x, size_y );
// reset our maps to have no information
resetMaps( );
}
124 void Costmap2D::resetMaps( )
{
std::unique_lock<mutex_t> lock( *access_ );
memset( costmap_, default_value_, size_x_ * size_y_ * sizeof( unsigned char ) );
}
130 void Costmap2D::resetMap( unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn )
{
resetMapToValue( x0, y0, xn, yn, default_value_ );
}
135 void Costmap2D::resetMapToValue(
unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn, unsigned char value )
{
std::unique_lock<mutex_t> lock( *( access_ ) );
unsigned int len = xn - x0;
for ( unsigned int y = y0 * size_x_ + x0; y < yn * size_x_ + x0; y += size_x_ ) {
memset( costmap_ + y, value, len * sizeof( unsigned char ) );
}
}
145 bool Costmap2D::copyCostmapWindow(
const Costmap2D & map, double win_origin_x, double win_origin_y,
double win_size_x,
double win_size_y )
{
// check for self windowing
if ( this == &map ) {
// ROS_ERROR( "Cannot convert this costmap into a window of itself" );
return false;
}
// clean up old data
deleteMaps( );
// compute the bounds of our new map
unsigned int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
if ( !map.worldToMap( win_origin_x, win_origin_y, lower_left_x, lower_left_y ) ||
!map.worldToMap(
win_origin_x + win_size_x, win_origin_y + win_size_y, upper_right_x,
upper_right_y ) )
{
// ROS_ERROR( "Cannot window a map that the window bounds don't fit inside of" );
return false;
}
size_x_ = upper_right_x - lower_left_x;
size_y_ = upper_right_y - lower_left_y;
resolution_ = map.resolution_;
origin_x_ = win_origin_x;
origin_y_ = win_origin_y;
// initialize our various maps and reset markers for inflation
initMaps( size_x_, size_y_ );
// copy the window of the static map and the costmap that we're taking
copyMapRegion(
map.costmap_, lower_left_x, lower_left_y, map.size_x_, costmap_, 0, 0, size_x_,
size_x_,
size_y_ );
return true;
}
187 bool Costmap2D::copyWindow(
const Costmap2D & source,
unsigned int sx0, unsigned int sy0, unsigned int sxn, unsigned int syn,
unsigned int dx0, unsigned int dy0 )
{
const unsigned int sz_x = sxn - sx0;
const unsigned int sz_y = syn - sy0;
if ( sxn > source.getSizeInCellsX( ) || syn > source.getSizeInCellsY( ) ) {
return false;
}
if ( dx0 + sz_x > size_x_ || dy0 + sz_y > size_y_ ) {
return false;
}
copyMapRegion(
source.costmap_, sx0, sy0, source.size_x_,
costmap_, dx0, dy0, size_x_,
sz_x, sz_y );
return true;
}
210 Costmap2D & Costmap2D::operator=( const Costmap2D & map )
{
// check for self assignement
if ( this == &map ) {
return *this;
}
// clean up old data
deleteMaps( );
size_x_ = map.size_x_;
size_y_ = map.size_y_;
resolution_ = map.resolution_;
origin_x_ = map.origin_x_;
origin_y_ = map.origin_y_;
// initialize our various maps
initMaps( size_x_, size_y_ );
// copy the cost map
memcpy( costmap_, map.costmap_, size_x_ * size_y_ * sizeof( unsigned char ) );
return *this;
}
235 Costmap2D::Costmap2D( const Costmap2D & map )
: costmap_( NULL )
{
access_ = new mutex_t( );
*this = map;
}
// just initialize everything to NULL by default
243 Costmap2D::Costmap2D( )
: size_x_( 0 ), size_y_( 0 ), resolution_( 0.0 ), origin_x_( 0.0 ), origin_y_( 0.0 ), costmap_( NULL )
{
access_ = new mutex_t( );
}
249 Costmap2D::~Costmap2D( )
{
deleteMaps( );
delete access_;
}
255 unsigned int Costmap2D::cellDistance( double world_dist )
{
double cells_dist = std::max( 0.0, ceil( world_dist / resolution_ ) );
return ( unsigned int )cells_dist;
}
261 unsigned char * Costmap2D::getCharMap( ) const
{
return costmap_;
}
266 unsigned char Costmap2D::getCost( unsigned int mx, unsigned int my ) const
{
return costmap_[getIndex( mx, my )];
}
271 unsigned char Costmap2D::getCost( unsigned int undex ) const
{
return costmap_[undex];
}
276 void Costmap2D::setCost( unsigned int mx, unsigned int my, unsigned char cost )
{
costmap_[getIndex( mx, my )] = cost;
}
281 void Costmap2D::mapToWorld( unsigned int mx, unsigned int my, double & wx, double & wy ) const
{
wx = origin_x_ + ( mx + 0.5 ) * resolution_;
wy = origin_y_ + ( my + 0.5 ) * resolution_;
}
287 bool Costmap2D::worldToMap( double wx, double wy, unsigned int & mx, unsigned int & my ) const
{
if ( wx < origin_x_ || wy < origin_y_ ) {
return false;
}
mx = static_cast<unsigned int>( ( wx - origin_x_ ) / resolution_ );
my = static_cast<unsigned int>( ( wy - origin_y_ ) / resolution_ );
if ( mx < size_x_ && my < size_y_ ) {
return true;
}
return false;
}
302 void Costmap2D::worldToMapNoBounds( double wx, double wy, int & mx, int & my ) const
{
mx = static_cast<int>( ( wx - origin_x_ ) / resolution_ );
my = static_cast<int>( ( wy - origin_y_ ) / resolution_ );
}
308 void Costmap2D::worldToMapEnforceBounds( double wx, double wy, int & mx, int & my ) const
{
// Here we avoid doing any math to wx, wy before comparing them to
// the bounds, so their values can go out to the max and min values
// of double floating point.
if ( wx < origin_x_ ) {
mx = 0;
} else if ( wx > resolution_ * size_x_ + origin_x_ ) {
mx = size_x_ - 1;
} else {
mx = static_cast<int>( ( wx - origin_x_ ) / resolution_ );
}
if ( wy < origin_y_ ) {
my = 0;
} else if ( wy > resolution_ * size_y_ + origin_y_ ) {
my = size_y_ - 1;
} else {
my = static_cast<int>( ( wy - origin_y_ ) / resolution_ );
}
}
330 void Costmap2D::updateOrigin( double new_origin_x, double new_origin_y )
{
// project the new origin into the grid
int cell_ox, cell_oy;
cell_ox = static_cast<int>( ( new_origin_x - origin_x_ ) / resolution_ );
cell_oy = static_cast<int>( ( new_origin_y - origin_y_ ) / resolution_ );
// compute the associated world coordinates for the origin cell
// because we want to keep things grid-aligned
double new_grid_ox, new_grid_oy;
new_grid_ox = origin_x_ + cell_ox * resolution_;
new_grid_oy = origin_y_ + cell_oy * resolution_;
// To save casting from unsigned int to int a bunch of times
int size_x = size_x_;
int size_y = size_y_;
// we need to compute the overlap of the new and existing windows
int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
lower_left_x = std::min( std::max( cell_ox, 0 ), size_x );
lower_left_y = std::min( std::max( cell_oy, 0 ), size_y );
upper_right_x = std::min( std::max( cell_ox + size_x, 0 ), size_x );
upper_right_y = std::min( std::max( cell_oy + size_y, 0 ), size_y );
unsigned int cell_size_x = upper_right_x - lower_left_x;
unsigned int cell_size_y = upper_right_y - lower_left_y;
// we need a map to store the obstacles in the window temporarily
unsigned char * local_map = new unsigned char[cell_size_x * cell_size_y];
// copy the local window in the costmap to the local map
copyMapRegion(
costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x,
cell_size_x,
cell_size_y );
// now we'll set the costmap to be completely unknown if we track unknown space
resetMaps( );
// update the origin with the appropriate world coordinates
origin_x_ = new_grid_ox;
origin_y_ = new_grid_oy;
// compute the starting cell location for copying data back in
int start_x = lower_left_x - cell_ox;
int start_y = lower_left_y - cell_oy;
// now we want to copy the overlapping information back into the map, but in its new location
copyMapRegion(
local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x,
cell_size_y );
// make sure to clean up
delete[] local_map;
}
386 bool Costmap2D::setConvexPolygonCost(
387 const std::vector<geometry_msgs::msg::Point> & polygon,
unsigned char cost_value )
{
// we assume the polygon is given in the global_frame...
// we need to transform it to map coordinates
std::vector<MapLocation> map_polygon;
for ( unsigned int i = 0; i < polygon.size( ); ++i ) {
MapLocation loc;
if ( !worldToMap( polygon[i].x, polygon[i].y, loc.x, loc.y ) ) {
// ( "Polygon lies outside map bounds, so we can't fill it" );
return false;
}
map_polygon.push_back( loc );
}
std::vector<MapLocation> polygon_cells;
// get the cells that fill the polygon
convexFillCells( map_polygon, polygon_cells );
// set the cost of those cells
for ( unsigned int i = 0; i < polygon_cells.size( ); ++i ) {
unsigned int index = getIndex( polygon_cells[i].x, polygon_cells[i].y );
costmap_[index] = cost_value;
}
return true;
}
415 void Costmap2D::polygonOutlineCells(
416 const std::vector<MapLocation> & polygon,
417 std::vector<MapLocation> & polygon_cells )
{
PolygonOutlineCells cell_gatherer( *this, costmap_, polygon_cells );
for ( unsigned int i = 0; i < polygon.size( ) - 1; ++i ) {
raytraceLine( cell_gatherer, polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y );
}
if ( !polygon.empty( ) ) {
unsigned int last_index = polygon.size( ) - 1;
// we also need to close the polygon by going from the last point to the first
raytraceLine(
cell_gatherer, polygon[last_index].x, polygon[last_index].y, polygon[0].x,
polygon[0].y );
}
}
432 void Costmap2D::convexFillCells(
433 const std::vector<MapLocation> & polygon,
434 std::vector<MapLocation> & polygon_cells )
{
// we need a minimum polygon of a triangle
if ( polygon.size( ) < 3 ) {
return;
}
// first get the cells that make up the outline of the polygon
polygonOutlineCells( polygon, polygon_cells );
// quick bubble sort to sort points by x
MapLocation swap;
unsigned int i = 0;
while ( i < polygon_cells.size( ) - 1 ) {
if ( polygon_cells[i].x > polygon_cells[i + 1].x ) {
swap = polygon_cells[i];
polygon_cells[i] = polygon_cells[i + 1];
polygon_cells[i + 1] = swap;
if ( i > 0 ) {
--i;
}
} else {
++i;
}
}
i = 0;
MapLocation min_pt;
MapLocation max_pt;
unsigned int min_x = polygon_cells[0].x;
unsigned int max_x = polygon_cells[polygon_cells.size( ) - 1].x;
// walk through each column and mark cells inside the polygon
for ( unsigned int x = min_x; x <= max_x; ++x ) {
if ( i >= polygon_cells.size( ) - 1 ) {
break;
}
if ( polygon_cells[i].y < polygon_cells[i + 1].y ) {
min_pt = polygon_cells[i];
max_pt = polygon_cells[i + 1];
} else {
min_pt = polygon_cells[i + 1];
max_pt = polygon_cells[i];
}
i += 2;
while ( i < polygon_cells.size( ) && polygon_cells[i].x == x ) {
if ( polygon_cells[i].y < min_pt.y ) {
min_pt = polygon_cells[i];
} else if ( polygon_cells[i].y > max_pt.y ) {
max_pt = polygon_cells[i];
}
++i;
}
MapLocation pt;
// loop though cells in the column
for ( unsigned int y = min_pt.y; y <= max_pt.y; ++y ) {
pt.x = x;
pt.y = y;
polygon_cells.push_back( pt );
}
}
}
501 unsigned int Costmap2D::getSizeInCellsX( ) const
{
return size_x_;
}
506 unsigned int Costmap2D::getSizeInCellsY( ) const
{
return size_y_;
}
511 double Costmap2D::getSizeInMetersX( ) const
{
return ( size_x_ - 1 + 0.5 ) * resolution_;
}
516 double Costmap2D::getSizeInMetersY( ) const
{
return ( size_y_ - 1 + 0.5 ) * resolution_;
}
521 double Costmap2D::getOriginX( ) const
{
return origin_x_;
}
526 double Costmap2D::getOriginY( ) const
{
return origin_y_;
}
531 double Costmap2D::getResolution( ) const
{
return resolution_;
}
536 bool Costmap2D::saveMap( std::string file_name )
{
FILE * fp = fopen( file_name.c_str( ), "w" );
if ( !fp ) {
return false;
}
fprintf( fp, "P2\n%u\n%u\n%u\n", size_x_, size_y_, 0xff );
for ( unsigned int iy = 0; iy < size_y_; iy++ ) {
for ( unsigned int ix = 0; ix < size_x_; ix++ ) {
unsigned char cost = getCost( ix, iy );
fprintf( fp, "%d ", cost );
}
fprintf( fp, "\n" );
}
fclose( fp );
return true;
}
} // namespace nav2_costmap_2d
1 /*
* Copyright ( C ) 2009, Willow Garage, Inc.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES ( INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE )
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <string>
#include <vector>
#include <memory>
#include <utility>
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "sensor_msgs/point_cloud2_iterator.hpp"
#include "nav2_voxel_grid/voxel_grid.hpp"
#include "nav2_msgs/msg/voxel_grid.hpp"
#include "nav2_util/execution_timer.hpp"
40 static inline void mapToWorld3D(
const unsigned int mx,
const unsigned int my, const unsigned int mz,
const double origin_x, const double origin_y, const double origin_z,
const double x_resolution, const double y_resolution,
const double z_resolution,
double & wx, double & wy, double & wz )
{
// returns the center point of the cell
wx = origin_x + ( mx + 0.5 ) * x_resolution;
wy = origin_y + ( my + 0.5 ) * y_resolution;
wz = origin_z + ( mz + 0.5 ) * z_resolution;
}
struct Cell
{
double x;
double y;
double z;
nav2_voxel_grid::VoxelStatus status;
};
typedef std::vector<Cell> V_Cell;
float g_colors_r[] = {0.0f, 0.0f, 1.0f};
float g_colors_g[] = {0.0f, 0.0f, 0.0f};
float g_colors_b[] = {0.0f, 1.0f, 0.0f};
float g_colors_a[] = {0.0f, 0.5f, 1.0f};
V_Cell g_marked;
V_Cell g_unknown;
rclcpp::Node::SharedPtr g_node;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_marked;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_unknown;
/**
* @brief An helper function to fill pointcloud2 of both the marked and unknown points from voxel_grid
* @param cloud PointCloud2 Ptr which needs to be filled
* @param num_channels Represents the total number of points that are going to be filled
* @param header Carries the header information that needs to be assigned to PointCloud2 header
* @param g_cells contains the x, y, z values that needs to be added to the PointCloud2
*/
83 void pointCloud2Helper(
std::unique_ptr<sensor_msgs::msg::PointCloud2> & cloud,
uint32_t num_channels,
std_msgs::msg::Header header,
V_Cell & g_cells )
{
cloud->header = header;
cloud->width = num_channels;
cloud->height = 1;
cloud->is_dense = true;
cloud->is_bigendian = false;
sensor_msgs::PointCloud2Modifier modifier( *cloud );
modifier.setPointCloud2Fields(
6, "x", 1, sensor_msgs::msg::PointField::FLOAT32,
"y", 1, sensor_msgs::msg::PointField::FLOAT32,
"z", 1, sensor_msgs::msg::PointField::FLOAT32,
"r", 1, sensor_msgs::msg::PointField::UINT8,
"g", 1, sensor_msgs::msg::PointField::UINT8,
"b", 1, sensor_msgs::msg::PointField::UINT8 );
sensor_msgs::PointCloud2Iterator<float> iter_x( *cloud, "x" );
sensor_msgs::PointCloud2Iterator<float> iter_y( *cloud, "y" );
sensor_msgs::PointCloud2Iterator<float> iter_z( *cloud, "z" );
sensor_msgs::PointCloud2Iterator<uint8_t> iter_r( *cloud, "r" );
sensor_msgs::PointCloud2Iterator<uint8_t> iter_g( *cloud, "g" );
sensor_msgs::PointCloud2Iterator<uint8_t> iter_b( *cloud, "b" );
for ( uint32_t i = 0; i < num_channels; ++i ) {
Cell & c = g_cells[i];
// assigning value to the point cloud2's iterator
*iter_x = c.x;
*iter_y = c.y;
*iter_z = c.z;
*iter_r = g_colors_r[c.status] * 255.0;
*iter_g = g_colors_g[c.status] * 255.0;
*iter_b = g_colors_b[c.status] * 255.0;
++iter_x;
++iter_y;
++iter_z;
++iter_r;
++iter_g;
++iter_b;
}
}
131 void voxelCallback( const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid )
{
if ( grid->data.empty( ) ) {
RCLCPP_ERROR( g_node->get_logger( ), "Received empty voxel grid" );
return;
}
nav2_util::ExecutionTimer timer;
timer.start( );
RCLCPP_DEBUG( g_node->get_logger( ), "Received voxel grid" );
const std::string frame_id = grid->header.frame_id;
const rclcpp::Time stamp = grid->header.stamp;
const uint32_t * data = &grid->data.front( );
const double x_origin = grid->origin.x;
const double y_origin = grid->origin.y;
const double z_origin = grid->origin.z;
const double x_res = grid->resolutions.x;
const double y_res = grid->resolutions.y;
const double z_res = grid->resolutions.z;
const uint32_t x_size = grid->size_x;
const uint32_t y_size = grid->size_y;
const uint32_t z_size = grid->size_z;
g_marked.clear( );
g_unknown.clear( );
uint32_t num_marked = 0;
uint32_t num_unknown = 0;
for ( uint32_t y_grid = 0; y_grid < y_size; ++y_grid ) {
for ( uint32_t x_grid = 0; x_grid < x_size; ++x_grid ) {
for ( uint32_t z_grid = 0; z_grid < z_size; ++z_grid ) {
nav2_voxel_grid::VoxelStatus status =
nav2_voxel_grid::VoxelGrid::getVoxel(
x_grid, y_grid,
z_grid, x_size, y_size, z_size, data );
if ( status == nav2_voxel_grid::UNKNOWN ) {
Cell c;
c.status = status;
mapToWorld3D(
x_grid, y_grid, z_grid, x_origin, y_origin,
z_origin, x_res, y_res, z_res, c.x, c.y, c.z );
g_unknown.push_back( c );
++num_unknown;
} else if ( status == nav2_voxel_grid::MARKED ) {
Cell c;
c.status = status;
mapToWorld3D(
x_grid, y_grid, z_grid, x_origin, y_origin,
z_origin, x_res, y_res, z_res, c.x, c.y, c.z );
g_marked.push_back( c );
++num_marked;
}
}
}
}
std_msgs::msg::Header pcl_header;
pcl_header.frame_id = frame_id;
pcl_header.stamp = stamp;
{
auto cloud = std::make_unique<sensor_msgs::msg::PointCloud2>( );
pointCloud2Helper( cloud, num_marked, pcl_header, g_marked );
pub_marked->publish( std::move( cloud ) );
}
{
auto cloud = std::make_unique<sensor_msgs::msg::PointCloud2>( );
pointCloud2Helper( cloud, num_unknown, pcl_header, g_unknown );
pub_unknown->publish( std::move( cloud ) );
}
timer.end( );
RCLCPP_DEBUG(
g_node->get_logger( ), "Published %d points in %f seconds",
num_marked + num_unknown, timer.elapsed_time_in_seconds( ) );
}
213 int main( int argc, char ** argv )
{
rclcpp::init( argc, argv );
g_node = rclcpp::Node::make_shared( "costmap_2d_cloud" );
RCLCPP_DEBUG( g_node->get_logger( ), "Starting up costmap_2d_cloud" );
pub_marked = g_node->create_publisher<sensor_msgs::msg::PointCloud2>(
"voxel_marked_cloud", 1 );
pub_unknown = g_node->create_publisher<sensor_msgs::msg::PointCloud2>(
"voxel_unknown_cloud", 1 );
auto sub = g_node->create_subscription<nav2_msgs::msg::VoxelGrid>(
"voxel_grid", rclcpp::SystemDefaultsQoS( ), voxelCallback );
rclcpp::spin( g_node->get_node_base_interface( ) );
rclcpp::shutdown( );
return 0;
}
1 /*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
* Steve Macenski
*********************************************************************/
#include <string>
#include <vector>
#include <memory>
#include <utility>
#include "rclcpp/rclcpp.hpp"
#include "visualization_msgs/msg/marker.hpp"
#include "nav2_msgs/msg/voxel_grid.hpp"
#include "nav2_voxel_grid/voxel_grid.hpp"
#include "nav2_util/execution_timer.hpp"
struct Cell
{
double x;
double y;
double z;
nav2_voxel_grid::VoxelStatus status;
};
typedef std::vector<Cell> V_Cell;
float g_colors_r[] = {0.0f, 0.0f, 1.0f};
float g_colors_g[] = {0.0f, 0.0f, 0.0f};
float g_colors_b[] = {0.0f, 1.0f, 0.0f};
float g_colors_a[] = {0.0f, 0.5f, 1.0f};
V_Cell g_cells;
rclcpp::Node::SharedPtr g_node;
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr pub;
68 void voxelCallback( const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid )
{
if ( grid->data.empty( ) ) {
RCLCPP_ERROR( g_node->get_logger( ), "Received voxel grid" );
return;
}
nav2_util::ExecutionTimer timer;
timer.start( );
RCLCPP_DEBUG( g_node->get_logger( ), "Received voxel grid" );
const std::string frame_id = grid->header.frame_id;
const rclcpp::Time stamp = grid->header.stamp;
const uint32_t * data = &grid->data.front( );
const double x_origin = grid->origin.x;
const double y_origin = grid->origin.y;
const double z_origin = grid->origin.z;
const double x_res = grid->resolutions.x;
const double y_res = grid->resolutions.y;
const double z_res = grid->resolutions.z;
const uint32_t x_size = grid->size_x;
const uint32_t y_size = grid->size_y;
const uint32_t z_size = grid->size_z;
g_cells.clear( );
uint32_t num_markers = 0;
for ( uint32_t y_grid = 0; y_grid < y_size; ++y_grid ) {
for ( uint32_t x_grid = 0; x_grid < x_size; ++x_grid ) {
for ( uint32_t z_grid = 0; z_grid < z_size; ++z_grid ) {
nav2_voxel_grid::VoxelStatus status =
nav2_voxel_grid::VoxelGrid::getVoxel(
x_grid, y_grid,
z_grid, x_size, y_size, z_size, data );
if ( status == nav2_voxel_grid::MARKED ) {
Cell c;
c.status = status;
c.x = x_origin + ( x_grid + 0.5 ) * x_res;
c.y = y_origin + ( y_grid + 0.5 ) * y_res;
c.z = z_origin + ( z_grid + 0.5 ) * z_res;
g_cells.push_back( c );
++num_markers;
}
}
}
}
auto m = std::make_unique<visualization_msgs::msg::Marker>( );
m->header.frame_id = frame_id;
m->header.stamp = stamp;
m->ns = g_node->get_namespace( );
m->id = 0;
m->type = visualization_msgs::msg::Marker::CUBE_LIST;
m->action = visualization_msgs::msg::Marker::ADD;
m->pose.orientation.w = 1.0;
m->scale.x = x_res;
m->scale.y = y_res;
m->scale.z = z_res;
m->color.r = g_colors_r[nav2_voxel_grid::MARKED];
m->color.g = g_colors_g[nav2_voxel_grid::MARKED];
m->color.b = g_colors_b[nav2_voxel_grid::MARKED];
m->color.a = g_colors_a[nav2_voxel_grid::MARKED];
m->points.resize( num_markers );
for ( uint32_t i = 0; i < num_markers; ++i ) {
Cell & c = g_cells[i];
geometry_msgs::msg::Point & p = m->points[i];
p.x = c.x;
p.y = c.y;
p.z = c.z;
}
pub->publish( std::move( m ) );
timer.end( );
RCLCPP_INFO(
g_node->get_logger( ), "Published %d markers in %f seconds",
num_markers, timer.elapsed_time_in_seconds( ) );
}
148 int main( int argc, char ** argv )
{
rclcpp::init( argc, argv );
g_node = rclcpp::Node::make_shared( "costmap_2d_marker" );
RCLCPP_DEBUG( g_node->get_logger( ), "Starting costmap_2d_marker" );
pub = g_node->create_publisher<visualization_msgs::msg::Marker>(
"visualization_marker", 1 );
auto sub = g_node->create_subscription<nav2_msgs::msg::VoxelGrid>(
"voxel_grid", rclcpp::SystemDefaultsQoS( ), voxelCallback );
rclcpp::spin( g_node->get_node_base_interface( ) );
}
1 /*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include <memory>
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "rclcpp/rclcpp.hpp"
43 int main( int argc, char ** argv )
{
rclcpp::init( argc, argv );
auto node = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "costmap" );
rclcpp::spin( node->get_node_base_interface( ) );
rclcpp::shutdown( );
return 0;
}
1 /*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* Copyright ( c ) 2019, Samsung Research America, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include "nav2_costmap_2d/costmap_2d_publisher.hpp"
#include <string>
#include <memory>
#include <utility>
#include "nav2_costmap_2d/cost_values.hpp"
namespace nav2_costmap_2d
{
char * Costmap2DPublisher::cost_translation_table_ = NULL;
52 Costmap2DPublisher::Costmap2DPublisher(
53 const nav2_util::LifecycleNode::WeakPtr & parent,
54 Costmap2D * costmap,
55 std::string global_frame,
56 std::string topic_name,
57 bool always_send_full_costmap )
: costmap_( costmap ),
global_frame_( global_frame ),
topic_name_( topic_name ),
active_( false ),
always_send_full_costmap_( always_send_full_costmap )
{
auto node = parent.lock( );
clock_ = node->get_clock( );
logger_ = node->get_logger( );
auto custom_qos = rclcpp::QoS( rclcpp::KeepLast( 1 ) ).transient_local( ).reliable( );
// TODO( bpwilcox ): port onNewSubscription functionality for publisher
costmap_pub_ = node->create_publisher<nav_msgs::msg::OccupancyGrid>(
topic_name,
custom_qos );
costmap_raw_pub_ = node->create_publisher<nav2_msgs::msg::Costmap>(
topic_name + "_raw",
custom_qos );
costmap_update_pub_ = node->create_publisher<map_msgs::msg::OccupancyGridUpdate>(
topic_name + "_updates", custom_qos );
// Create a service that will use the callback function to handle requests.
costmap_service_ = node->create_service<nav2_msgs::srv::GetCostmap>(
"get_costmap", std::bind(
&Costmap2DPublisher::costmap_service_callback,
this, std::placeholders::_1, std::placeholders::_2,
std::placeholders::_3 ) );
if ( cost_translation_table_ == NULL ) {
cost_translation_table_ = new char[256];
// special values:
cost_translation_table_[0] = 0; // NO obstacle
cost_translation_table_[253] = 99; // INSCRIBED obstacle
cost_translation_table_[254] = 100; // LETHAL obstacle
cost_translation_table_[255] = -1; // UNKNOWN
// regular cost values scale the range 1 to 252 ( inclusive ) to fit
// into 1 to 98 ( inclusive ).
for ( int i = 1; i < 253; i++ ) {
cost_translation_table_[i] = static_cast<char>( 1 + ( 97 * ( i - 1 ) ) / 251 );
}
}
xn_ = yn_ = 0;
x0_ = costmap_->getSizeInCellsX( );
y0_ = costmap_->getSizeInCellsY( );
}
108 Costmap2DPublisher::~Costmap2DPublisher( ) {}
// TODO( bpwilcox ): find equivalent/workaround to ros::SingleSubscriberPublishr
/*
void Costmap2DPublisher::onNewSubscription( const ros::SingleSubscriberPublisher& pub )
{
prepareGrid( );
pub.publish( grid_ );
} */
// prepare grid_ message for publication.
119 void Costmap2DPublisher::prepareGrid( )
{
std::unique_lock<Costmap2D::mutex_t> lock( *( costmap_->getMutex( ) ) );
grid_resolution = costmap_->getResolution( );
grid_width = costmap_->getSizeInCellsX( );
grid_height = costmap_->getSizeInCellsY( );
grid_ = std::make_unique<nav_msgs::msg::OccupancyGrid>( );
grid_->header.frame_id = global_frame_;
grid_->header.stamp = clock_->now( );
grid_->info.resolution = grid_resolution;
grid_->info.width = grid_width;
grid_->info.height = grid_height;
double wx, wy;
costmap_->mapToWorld( 0, 0, wx, wy );
grid_->info.origin.position.x = wx - grid_resolution / 2;
grid_->info.origin.position.y = wy - grid_resolution / 2;
grid_->info.origin.position.z = 0.0;
grid_->info.origin.orientation.w = 1.0;
saved_origin_x_ = costmap_->getOriginX( );
saved_origin_y_ = costmap_->getOriginY( );
grid_->data.resize( grid_->info.width * grid_->info.height );
unsigned char * data = costmap_->getCharMap( );
for ( unsigned int i = 0; i < grid_->data.size( ); i++ ) {
grid_->data[i] = cost_translation_table_[data[i]];
}
}
153 void Costmap2DPublisher::prepareCostmap( )
{
std::unique_lock<Costmap2D::mutex_t> lock( *( costmap_->getMutex( ) ) );
double resolution = costmap_->getResolution( );
costmap_raw_ = std::make_unique<nav2_msgs::msg::Costmap>( );
costmap_raw_->header.frame_id = global_frame_;
costmap_raw_->header.stamp = clock_->now( );
costmap_raw_->metadata.layer = "master";
costmap_raw_->metadata.resolution = resolution;
costmap_raw_->metadata.size_x = costmap_->getSizeInCellsX( );
costmap_raw_->metadata.size_y = costmap_->getSizeInCellsY( );
double wx, wy;
costmap_->mapToWorld( 0, 0, wx, wy );
costmap_raw_->metadata.origin.position.x = wx - resolution / 2;
costmap_raw_->metadata.origin.position.y = wy - resolution / 2;
costmap_raw_->metadata.origin.position.z = 0.0;
costmap_raw_->metadata.origin.orientation.w = 1.0;
costmap_raw_->data.resize( costmap_raw_->metadata.size_x * costmap_raw_->metadata.size_y );
unsigned char * data = costmap_->getCharMap( );
for ( unsigned int i = 0; i < costmap_raw_->data.size( ); i++ ) {
costmap_raw_->data[i] = data[i];
}
}
184 void Costmap2DPublisher::publishCostmap( )
{
if ( costmap_raw_pub_->get_subscription_count( ) > 0 ) {
prepareCostmap( );
costmap_raw_pub_->publish( std::move( costmap_raw_ ) );
}
float resolution = costmap_->getResolution( );
if ( always_send_full_costmap_ || grid_resolution != resolution ||
grid_width != costmap_->getSizeInCellsX( ) ||
grid_height != costmap_->getSizeInCellsY( ) ||
saved_origin_x_ != costmap_->getOriginX( ) ||
saved_origin_y_ != costmap_->getOriginY( ) )
{
if ( costmap_pub_->get_subscription_count( ) > 0 ) {
prepareGrid( );
costmap_pub_->publish( std::move( grid_ ) );
}
} else if ( x0_ < xn_ ) {
if ( costmap_update_pub_->get_subscription_count( ) > 0 ) {
std::unique_lock<Costmap2D::mutex_t> lock( *( costmap_->getMutex( ) ) );
// Publish Just an Update
auto update = std::make_unique<map_msgs::msg::OccupancyGridUpdate>( );
update->header.stamp = rclcpp::Time( );
update->header.frame_id = global_frame_;
update->x = x0_;
update->y = y0_;
update->width = xn_ - x0_;
update->height = yn_ - y0_;
update->data.resize( update->width * update->height );
unsigned int i = 0;
for ( unsigned int y = y0_; y < yn_; y++ ) {
for ( unsigned int x = x0_; x < xn_; x++ ) {
unsigned char cost = costmap_->getCost( x, y );
update->data[i++] = cost_translation_table_[cost];
}
}
costmap_update_pub_->publish( std::move( update ) );
}
}
xn_ = yn_ = 0;
x0_ = costmap_->getSizeInCellsX( );
y0_ = costmap_->getSizeInCellsY( );
}
void
231 Costmap2DPublisher::costmap_service_callback(
232 const std::shared_ptr<rmw_request_id_t>/*request_header*/,
233 const std::shared_ptr<nav2_msgs::srv::GetCostmap::Request>/*request*/,
234 const std::shared_ptr<nav2_msgs::srv::GetCostmap::Response> response )
{
RCLCPP_DEBUG( logger_, "Received costmap service request" );
// TODO( bpwilcox ): Grab correct orientation information
tf2::Quaternion quaternion;
quaternion.setRPY( 0.0, 0.0, 0.0 );
auto size_x = costmap_->getSizeInCellsX( );
auto size_y = costmap_->getSizeInCellsY( );
auto data_length = size_x * size_y;
unsigned char * data = costmap_->getCharMap( );
auto current_time = clock_->now( );
response->map.header.stamp = current_time;
response->map.header.frame_id = global_frame_;
response->map.metadata.size_x = size_x;
response->map.metadata.size_y = size_y;
response->map.metadata.resolution = costmap_->getResolution( );
response->map.metadata.layer = "master";
response->map.metadata.map_load_time = current_time;
response->map.metadata.update_time = current_time;
response->map.metadata.origin.position.x = costmap_->getOriginX( );
response->map.metadata.origin.position.y = costmap_->getOriginY( );
response->map.metadata.origin.position.z = 0.0;
response->map.metadata.origin.orientation = tf2::toMsg( quaternion );
response->map.data.resize( data_length );
response->map.data.assign( data, data + data_length );
}
} // end namespace nav2_costmap_2d
/*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include <memory>
#include <chrono>
#include <string>
#include <vector>
#include <utility>
#include "nav2_costmap_2d/layered_costmap.hpp"
#include "nav2_util/execution_timer.hpp"
#include "nav2_util/node_utils.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/create_timer_ros.h"
#include "nav2_util/robot_utils.hpp"
#include "rcl_interfaces/msg/set_parameters_result.hpp"
using namespace std::chrono_literals;
using std::placeholders::_1;
using rcl_interfaces::msg::ParameterType;
namespace nav2_costmap_2d
{
61 Costmap2DROS::Costmap2DROS( const std::string & name )
: Costmap2DROS( name, "/", name ) {}
64 Costmap2DROS::Costmap2DROS(
65 const std::string & name,
66 const std::string & parent_namespace,
67 const std::string & local_namespace )
: nav2_util::LifecycleNode( name, "",
// NodeOption arguments take precedence over the ones provided on the command line
// use this to make sure the node is placed on the provided namespace
// TODO( orduno ) Pass a sub-node instead of creating a new node for better handling
// of the namespaces
rclcpp::NodeOptions( ).arguments( {
"--ros-args", "-r", std::string( "__ns:=" ) +
nav2_util::add_namespaces( parent_namespace, local_namespace ),
"--ros-args", "-r", name + ":" + std::string( "__node:=" ) + name
} ) ),
name_( name ),
parent_namespace_( parent_namespace ),
default_plugins_{"static_layer", "obstacle_layer", "inflation_layer"},
81 default_types_{
"nav2_costmap_2d::StaticLayer",
"nav2_costmap_2d::ObstacleLayer",
"nav2_costmap_2d::InflationLayer"}
{
86 RCLCPP_INFO( get_logger( ), "Creating Costmap" );
std::vector<std::string> clearable_layers{"obstacle_layer", "voxel_layer", "range_layer"};
declare_parameter( "always_send_full_costmap", rclcpp::ParameterValue( false ) );
declare_parameter( "footprint_padding", rclcpp::ParameterValue( 0.01f ) );
declare_parameter( "footprint", rclcpp::ParameterValue( std::string( "[]" ) ) );
declare_parameter( "global_frame", rclcpp::ParameterValue( std::string( "map" ) ) );
declare_parameter( "height", rclcpp::ParameterValue( 5 ) );
declare_parameter( "width", rclcpp::ParameterValue( 5 ) );
declare_parameter( "lethal_cost_threshold", rclcpp::ParameterValue( 100 ) );
declare_parameter(
"map_topic", rclcpp::ParameterValue(
( parent_namespace_ == "/" ? "/" : parent_namespace_ + "/" ) + std::string( "map" ) ) );
declare_parameter( "observation_sources", rclcpp::ParameterValue( std::string( "" ) ) );
declare_parameter( "origin_x", rclcpp::ParameterValue( 0.0 ) );
declare_parameter( "origin_y", rclcpp::ParameterValue( 0.0 ) );
declare_parameter( "plugins", rclcpp::ParameterValue( default_plugins_ ) );
declare_parameter( "filters", rclcpp::ParameterValue( std::vector<std::string>( ) ) );
declare_parameter( "publish_frequency", rclcpp::ParameterValue( 1.0 ) );
declare_parameter( "resolution", rclcpp::ParameterValue( 0.1 ) );
declare_parameter( "robot_base_frame", rclcpp::ParameterValue( std::string( "base_link" ) ) );
declare_parameter( "robot_radius", rclcpp::ParameterValue( 0.1 ) );
declare_parameter( "rolling_window", rclcpp::ParameterValue( false ) );
declare_parameter( "track_unknown_space", rclcpp::ParameterValue( false ) );
declare_parameter( "transform_tolerance", rclcpp::ParameterValue( 0.3 ) );
declare_parameter( "trinary_costmap", rclcpp::ParameterValue( true ) );
declare_parameter( "unknown_cost_value", rclcpp::ParameterValue( static_cast<unsigned char>( 0xff ) ) );
declare_parameter( "update_frequency", rclcpp::ParameterValue( 5.0 ) );
declare_parameter( "use_maximum", rclcpp::ParameterValue( false ) );
declare_parameter( "clearable_layers", rclcpp::ParameterValue( clearable_layers ) );
}
Costmap2DROS::~Costmap2DROS( )
{
}
nav2_util::CallbackReturn
Costmap2DROS::on_configure( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Configuring" );
getParameters( );
callback_group_ = create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive, false );
// Create the costmap itself
layered_costmap_ = std::make_unique<LayeredCostmap>(
global_frame_, rolling_window_, track_unknown_space_ );
if ( !layered_costmap_->isSizeLocked( ) ) {
layered_costmap_->resizeMap(
( unsigned int )( map_width_meters_ / resolution_ ),
( unsigned int )( map_height_meters_ / resolution_ ), resolution_, origin_x_, origin_y_ );
}
// Create the transform-related objects
tf_buffer_ = std::make_shared<tf2_ros::Buffer>( get_clock( ) );
auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
get_node_base_interface( ),
get_node_timers_interface( ),
callback_group_ );
tf_buffer_->setCreateTimerInterface( timer_interface );
tf_listener_ = std::make_shared<tf2_ros::TransformListener>( *tf_buffer_ );
// Then load and add the plug-ins to the costmap
for ( unsigned int i = 0; i < plugin_names_.size( ); ++i ) {
RCLCPP_INFO( get_logger( ), "Using plugin \"%s\"", plugin_names_[i].c_str( ) );
std::shared_ptr<Layer> plugin = plugin_loader_.createSharedInstance( plugin_types_[i] );
layered_costmap_->addPlugin( plugin );
// TODO( mjeronimo ): instead of get( ), use a shared ptr
plugin->initialize(
layered_costmap_.get( ), plugin_names_[i], tf_buffer_.get( ),
shared_from_this( ), callback_group_ );
RCLCPP_INFO( get_logger( ), "Initialized plugin \"%s\"", plugin_names_[i].c_str( ) );
}
// and costmap filters as well
for ( unsigned int i = 0; i < filter_names_.size( ); ++i ) {
RCLCPP_INFO( get_logger( ), "Using costmap filter \"%s\"", filter_names_[i].c_str( ) );
std::shared_ptr<Layer> filter = plugin_loader_.createSharedInstance( filter_types_[i] );
layered_costmap_->addFilter( filter );
filter->initialize(
layered_costmap_.get( ), filter_names_[i], tf_buffer_.get( ),
shared_from_this( ), callback_group_ );
RCLCPP_INFO( get_logger( ), "Initialized costmap filter \"%s\"", filter_names_[i].c_str( ) );
}
// Create the publishers and subscribers
footprint_sub_ = create_subscription<geometry_msgs::msg::Polygon>(
"footprint",
rclcpp::SystemDefaultsQoS( ),
std::bind( &Costmap2DROS::setRobotFootprintPolygon, this, std::placeholders::_1 ) );
footprint_pub_ = create_publisher<geometry_msgs::msg::PolygonStamped>(
"published_footprint", rclcpp::SystemDefaultsQoS( ) );
costmap_publisher_ = std::make_unique<Costmap2DPublisher>(
shared_from_this( ),
layered_costmap_->getCostmap( ), global_frame_,
"costmap", always_send_full_costmap_ );
// Set the footprint
if ( use_radius_ ) {
setRobotFootprint( makeFootprintFromRadius( robot_radius_ ) );
} else {
std::vector<geometry_msgs::msg::Point> new_footprint;
makeFootprintFromString( footprint_, new_footprint );
setRobotFootprint( new_footprint );
}
// Add cleaning service
clear_costmap_service_ = std::make_unique<ClearCostmapService>( shared_from_this( ), *this );
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>( );
executor_->add_callback_group( callback_group_, get_node_base_interface( ) );
executor_thread_ = std::make_unique<nav2_util::NodeThread>( executor_ );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
Costmap2DROS::on_activate( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Activating" );
costmap_publisher_->on_activate( );
footprint_pub_->on_activate( );
// First, make sure that the transform between the robot base frame
// and the global frame is available
std::string tf_error;
RCLCPP_INFO( get_logger( ), "Checking transform" );
rclcpp::Rate r( 2 );
while ( rclcpp::ok( ) &&
!tf_buffer_->canTransform(
global_frame_, robot_base_frame_, tf2::TimePointZero, &tf_error ) )
{
RCLCPP_INFO(
get_logger( ), "Timed out waiting for transform from %s to %s"
" to become available, tf error: %s",
robot_base_frame_.c_str( ), global_frame_.c_str( ), tf_error.c_str( ) );
// The error string will accumulate and errors will typically be the same, so the last
// will do for the warning above. Reset the string here to avoid accumulation
tf_error.clear( );
r.sleep( );
}
// Create a thread to handle updating the map
stopped_ = true; // to active plugins
stop_updates_ = false;
map_update_thread_shutdown_ = false;
map_update_thread_ = std::make_unique<std::thread>(
std::bind( &Costmap2DROS::mapUpdateLoop, this, map_update_frequency_ ) );
start( );
// Add callback for dynamic parameters
dyn_params_handler = this->add_on_set_parameters_callback(
std::bind( &Costmap2DROS::dynamicParametersCallback, this, _1 ) );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
Costmap2DROS::on_deactivate( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Deactivating" );
dyn_params_handler.reset( );
costmap_publisher_->on_deactivate( );
footprint_pub_->on_deactivate( );
stop( );
// Map thread stuff
map_update_thread_shutdown_ = true;
map_update_thread_->join( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
Costmap2DROS::on_cleanup( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Cleaning up" );
layered_costmap_.reset( );
tf_listener_.reset( );
tf_buffer_.reset( );
footprint_sub_.reset( );
footprint_pub_.reset( );
costmap_publisher_.reset( );
clear_costmap_service_.reset( );
executor_thread_.reset( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
Costmap2DROS::on_shutdown( const rclcpp_lifecycle::State & )
{
RCLCPP_INFO( get_logger( ), "Shutting down" );
return nav2_util::CallbackReturn::SUCCESS;
}
void
Costmap2DROS::getParameters( )
{
RCLCPP_DEBUG( get_logger( ), " getParameters" );
// Get all of the required parameters
get_parameter( "always_send_full_costmap", always_send_full_costmap_ );
get_parameter( "footprint", footprint_ );
get_parameter( "footprint_padding", footprint_padding_ );
get_parameter( "global_frame", global_frame_ );
get_parameter( "height", map_height_meters_ );
get_parameter( "origin_x", origin_x_ );
get_parameter( "origin_y", origin_y_ );
get_parameter( "publish_frequency", map_publish_frequency_ );
get_parameter( "resolution", resolution_ );
get_parameter( "robot_base_frame", robot_base_frame_ );
get_parameter( "robot_radius", robot_radius_ );
get_parameter( "rolling_window", rolling_window_ );
get_parameter( "track_unknown_space", track_unknown_space_ );
get_parameter( "transform_tolerance", transform_tolerance_ );
get_parameter( "update_frequency", map_update_frequency_ );
get_parameter( "width", map_width_meters_ );
get_parameter( "plugins", plugin_names_ );
get_parameter( "filters", filter_names_ );
auto node = shared_from_this( );
if ( plugin_names_ == default_plugins_ ) {
for ( size_t i = 0; i < default_plugins_.size( ); ++i ) {
nav2_util::declare_parameter_if_not_declared(
node, default_plugins_[i] + ".plugin", rclcpp::ParameterValue( default_types_[i] ) );
}
}
plugin_types_.resize( plugin_names_.size( ) );
filter_types_.resize( filter_names_.size( ) );
// 1. All plugins must have 'plugin' param defined in their namespace to define the plugin type
for ( size_t i = 0; i < plugin_names_.size( ); ++i ) {
plugin_types_[i] = nav2_util::get_plugin_type_param( node, plugin_names_[i] );
}
for ( size_t i = 0; i < filter_names_.size( ); ++i ) {
filter_types_[i] = nav2_util::get_plugin_type_param( node, filter_names_[i] );
}
// 2. The map publish frequency cannot be 0 ( to avoid a divde-by-zero )
if ( map_publish_frequency_ > 0 ) {
publish_cycle_ = rclcpp::Duration::from_seconds( 1 / map_publish_frequency_ );
} else {
publish_cycle_ = rclcpp::Duration( -1s );
}
// 3. If the footprint has been specified, it must be in the correct format
use_radius_ = true;
if ( footprint_ != "" && footprint_ != "[]" ) {
// Footprint parameter has been specified, try to convert it
std::vector<geometry_msgs::msg::Point> new_footprint;
if ( makeFootprintFromString( footprint_, new_footprint ) ) {
// The specified footprint is valid, so we'll use that instead of the radius
use_radius_ = false;
} else {
// Footprint provided but invalid, so stay with the radius
RCLCPP_ERROR(
get_logger( ), "The footprint parameter is invalid: \"%s\", using radius ( %lf ) instead",
footprint_.c_str( ), robot_radius_ );
}
}
}
void
Costmap2DROS::setRobotFootprint( const std::vector<geometry_msgs::msg::Point> & points )
{
unpadded_footprint_ = points;
padded_footprint_ = points;
padFootprint( padded_footprint_, footprint_padding_ );
layered_costmap_->setFootprint( padded_footprint_ );
}
void
Costmap2DROS::setRobotFootprintPolygon(
const geometry_msgs::msg::Polygon::SharedPtr footprint )
{
setRobotFootprint( toPointVector( footprint ) );
}
void
Costmap2DROS::getOrientedFootprint( std::vector<geometry_msgs::msg::Point> & oriented_footprint )
{
geometry_msgs::msg::PoseStamped global_pose;
if ( !getRobotPose( global_pose ) ) {
return;
}
double yaw = tf2::getYaw( global_pose.pose.orientation );
transformFootprint(
global_pose.pose.position.x, global_pose.pose.position.y, yaw,
padded_footprint_, oriented_footprint );
}
void
Costmap2DROS::mapUpdateLoop( double frequency )
{
RCLCPP_DEBUG( get_logger( ), "mapUpdateLoop frequency: %lf", frequency );
// the user might not want to run the loop every cycle
if ( frequency == 0.0 ) {
return;
}
RCLCPP_DEBUG( get_logger( ), "Entering loop" );
rclcpp::WallRate r( frequency ); // 200ms by default
while ( rclcpp::ok( ) && !map_update_thread_shutdown_ ) {
nav2_util::ExecutionTimer timer;
// Measure the execution time of the updateMap method
timer.start( );
updateMap( );
timer.end( );
RCLCPP_DEBUG( get_logger( ), "Map update time: %.9f", timer.elapsed_time_in_seconds( ) );
if ( publish_cycle_ > rclcpp::Duration( 0s ) && layered_costmap_->isInitialized( ) ) {
unsigned int x0, y0, xn, yn;
layered_costmap_->getBounds( &x0, &xn, &y0, &yn );
costmap_publisher_->updateBounds( x0, xn, y0, yn );
auto current_time = now( );
if ( ( last_publish_ + publish_cycle_ < current_time ) || // publish_cycle_ is due
( current_time < last_publish_ ) ) // time has moved backwards, probably due to a switch to sim_time // NOLINT
{
RCLCPP_DEBUG( get_logger( ), "Publish costmap at %s", name_.c_str( ) );
costmap_publisher_->publishCostmap( );
last_publish_ = current_time;
}
}
// Make sure to sleep for the remainder of our cycle time
r.sleep( );
#if 0
// TODO( bpwilcox ): find ROS2 equivalent or port for r.cycletime( )
if ( r.period( ) > tf2::durationFromSec( 1 / frequency ) ) {
RCLCPP_WARN(
get_logger( ),
"Costmap2DROS: Map update loop missed its desired rate of %.4fHz... "
"the loop actually took %.4f seconds", frequency, r.period( ) );
}
#endif
}
}
void
Costmap2DROS::updateMap( )
{
RCLCPP_DEBUG( get_logger( ), "Updating map..." );
if ( !stop_updates_ ) {
// get global pose
geometry_msgs::msg::PoseStamped pose;
if ( getRobotPose( pose ) ) {
const double & x = pose.pose.position.x;
const double & y = pose.pose.position.y;
const double yaw = tf2::getYaw( pose.pose.orientation );
layered_costmap_->updateMap( x, y, yaw );
auto footprint = std::make_unique<geometry_msgs::msg::PolygonStamped>( );
footprint->header = pose.header;
transformFootprint( x, y, yaw, padded_footprint_, *footprint );
RCLCPP_DEBUG( get_logger( ), "Publishing footprint" );
footprint_pub_->publish( std::move( footprint ) );
initialized_ = true;
}
}
}
void
Costmap2DROS::start( )
{
RCLCPP_INFO( get_logger( ), "start" );
std::vector<std::shared_ptr<Layer>> * plugins = layered_costmap_->getPlugins( );
std::vector<std::shared_ptr<Layer>> * filters = layered_costmap_->getFilters( );
// check if we're stopped or just paused
if ( stopped_ ) {
// if we're stopped we need to re-subscribe to topics
for ( std::vector<std::shared_ptr<Layer>>::iterator plugin = plugins->begin( );
plugin != plugins->end( );
++plugin )
{
( *plugin )->activate( );
}
for ( std::vector<std::shared_ptr<Layer>>::iterator filter = filters->begin( );
filter != filters->end( );
++filter )
{
( *filter )->activate( );
}
stopped_ = false;
}
stop_updates_ = false;
// block until the costmap is re-initialized.. meaning one update cycle has run
rclcpp::Rate r( 20.0 );
while ( rclcpp::ok( ) && !initialized_ ) {
RCLCPP_DEBUG( get_logger( ), "Sleeping, waiting for initialized_" );
r.sleep( );
}
}
void
Costmap2DROS::stop( )
{
stop_updates_ = true;
std::vector<std::shared_ptr<Layer>> * plugins = layered_costmap_->getPlugins( );
std::vector<std::shared_ptr<Layer>> * filters = layered_costmap_->getFilters( );
// unsubscribe from topics
for ( std::vector<std::shared_ptr<Layer>>::iterator plugin = plugins->begin( );
plugin != plugins->end( ); ++plugin )
{
( *plugin )->deactivate( );
}
for ( std::vector<std::shared_ptr<Layer>>::iterator filter = filters->begin( );
filter != filters->end( ); ++filter )
{
( *filter )->deactivate( );
}
initialized_ = false;
stopped_ = true;
}
void
Costmap2DROS::pause( )
{
stop_updates_ = true;
initialized_ = false;
}
void
Costmap2DROS::resume( )
{
stop_updates_ = false;
// block until the costmap is re-initialized.. meaning one update cycle has run
rclcpp::Rate r( 100.0 );
while ( !initialized_ ) {
r.sleep( );
}
}
void
Costmap2DROS::resetLayers( )
{
Costmap2D * top = layered_costmap_->getCostmap( );
top->resetMap( 0, 0, top->getSizeInCellsX( ), top->getSizeInCellsY( ) );
// Reset each of the plugins
std::vector<std::shared_ptr<Layer>> * plugins = layered_costmap_->getPlugins( );
std::vector<std::shared_ptr<Layer>> * filters = layered_costmap_->getFilters( );
for ( std::vector<std::shared_ptr<Layer>>::iterator plugin = plugins->begin( );
plugin != plugins->end( ); ++plugin )
{
( *plugin )->reset( );
}
for ( std::vector<std::shared_ptr<Layer>>::iterator filter = filters->begin( );
filter != filters->end( ); ++filter )
{
( *filter )->reset( );
}
}
bool
Costmap2DROS::getRobotPose( geometry_msgs::msg::PoseStamped & global_pose )
{
return nav2_util::getCurrentPose(
global_pose, *tf_buffer_,
global_frame_, robot_base_frame_, transform_tolerance_ );
}
bool
Costmap2DROS::transformPoseToGlobalFrame(
const geometry_msgs::msg::PoseStamped & input_pose,
geometry_msgs::msg::PoseStamped & transformed_pose )
{
if ( input_pose.header.frame_id == global_frame_ ) {
transformed_pose = input_pose;
return true;
} else {
return nav2_util::transformPoseInTargetFrame(
input_pose, transformed_pose, *tf_buffer_,
global_frame_, transform_tolerance_ );
}
}
rcl_interfaces::msg::SetParametersResult
Costmap2DROS::dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters )
{
auto result = rcl_interfaces::msg::SetParametersResult( );
bool resize_map = false;
for ( auto parameter : parameters ) {
const auto & type = parameter.get_type( );
const auto & name = parameter.get_name( );
if ( type == ParameterType::PARAMETER_DOUBLE ) {
if ( name == "robot_radius" ) {
robot_radius_ = parameter.as_double( );
// Set the footprint
if ( use_radius_ ) {
setRobotFootprint( makeFootprintFromRadius( robot_radius_ ) );
}
} else if ( name == "footprint_padding" ) {
footprint_padding_ = parameter.as_double( );
padded_footprint_ = unpadded_footprint_;
padFootprint( padded_footprint_, footprint_padding_ );
layered_costmap_->setFootprint( padded_footprint_ );
} else if ( name == "transform_tolerance" ) {
transform_tolerance_ = parameter.as_double( );
} else if ( name == "publish_frequency" ) {
map_publish_frequency_ = parameter.as_double( );
if ( map_publish_frequency_ > 0 ) {
publish_cycle_ = rclcpp::Duration::from_seconds( 1 / map_publish_frequency_ );
} else {
publish_cycle_ = rclcpp::Duration( -1s );
}
} else if ( name == "resolution" ) {
resize_map = true;
resolution_ = parameter.as_double( );
} else if ( name == "origin_x" ) {
resize_map = true;
origin_x_ = parameter.as_double( );
} else if ( name == "origin_y" ) {
resize_map = true;
origin_y_ = parameter.as_double( );
}
} else if ( type == ParameterType::PARAMETER_INTEGER ) {
if ( name == "width" ) {
resize_map = true;
map_width_meters_ = parameter.as_int( );
} else if ( name == "height" ) {
resize_map = true;
map_height_meters_ = parameter.as_int( );
}
} else if ( type == ParameterType::PARAMETER_STRING ) {
if ( name == "footprint" ) {
footprint_ = parameter.as_string( );
std::vector<geometry_msgs::msg::Point> new_footprint;
if ( makeFootprintFromString( footprint_, new_footprint ) ) {
setRobotFootprint( new_footprint );
}
} else if ( name == "robot_base_frame" ) {
// First, make sure that the transform between the robot base frame
// and the global frame is available
std::string tf_error;
RCLCPP_INFO( get_logger( ), "Checking transform" );
if ( !tf_buffer_->canTransform(
global_frame_, parameter.as_string( ), tf2::TimePointZero,
tf2::durationFromSec( 1.0 ), &tf_error ) )
{
RCLCPP_WARN(
get_logger( ), "Timed out waiting for transform from %s to %s"
" to become available, tf error: %s",
parameter.as_string( ).c_str( ), global_frame_.c_str( ), tf_error.c_str( ) );
RCLCPP_WARN(
get_logger( ), "Rejecting robot_base_frame change to %s , leaving it to its original"
" value of %s", parameter.as_string( ).c_str( ), robot_base_frame_.c_str( ) );
result.successful = false;
return result;
}
robot_base_frame_ = parameter.as_string( );
}
}
}
if ( resize_map && !layered_costmap_->isSizeLocked( ) ) {
layered_costmap_->resizeMap(
( unsigned int )( map_width_meters_ / resolution_ ),
( unsigned int )( map_height_meters_ / resolution_ ), resolution_, origin_x_, origin_y_ );
}
result.successful = true;
return result;
}
} // namespace nav2_costmap_2d
1 /*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include <nav2_costmap_2d/costmap_layer.hpp>
#include <stdexcept>
#include <algorithm>
namespace nav2_costmap_2d
{
46 void CostmapLayer::touch(
double x, double y, double * min_x, double * min_y, double * max_x,
double * max_y )
{
*min_x = std::min( x, *min_x );
*min_y = std::min( y, *min_y );
*max_x = std::max( x, *max_x );
*max_y = std::max( y, *max_y );
}
56 void CostmapLayer::matchSize( )
{
Costmap2D * master = layered_costmap_->getCostmap( );
resizeMap(
master->getSizeInCellsX( ), master->getSizeInCellsY( ), master->getResolution( ),
master->getOriginX( ), master->getOriginY( ) );
}
64 void CostmapLayer::clearArea( int start_x, int start_y, int end_x, int end_y, bool invert )
{
current_ = false;
unsigned char * grid = getCharMap( );
for ( int x = 0; x < static_cast<int>( getSizeInCellsX( ) ); x++ ) {
bool xrange = x > start_x && x < end_x;
for ( int y = 0; y < static_cast<int>( getSizeInCellsY( ) ); y++ ) {
if ( ( xrange && y > start_y && y < end_y ) == invert ) {
continue;
}
int index = getIndex( x, y );
if ( grid[index] != NO_INFORMATION ) {
grid[index] = NO_INFORMATION;
}
}
}
}
83 void CostmapLayer::addExtraBounds( double mx0, double my0, double mx1, double my1 )
{
extra_min_x_ = std::min( mx0, extra_min_x_ );
extra_max_x_ = std::max( mx1, extra_max_x_ );
extra_min_y_ = std::min( my0, extra_min_y_ );
extra_max_y_ = std::max( my1, extra_max_y_ );
has_extra_bounds_ = true;
}
92 void CostmapLayer::useExtraBounds( double * min_x, double * min_y, double * max_x, double * max_y )
{
if ( !has_extra_bounds_ ) {
return;
}
*min_x = std::min( extra_min_x_, *min_x );
*min_y = std::min( extra_min_y_, *min_y );
*max_x = std::max( extra_max_x_, *max_x );
*max_y = std::max( extra_max_y_, *max_y );
extra_min_x_ = 1e6;
extra_min_y_ = 1e6;
extra_max_x_ = -1e6;
extra_max_y_ = -1e6;
has_extra_bounds_ = false;
}
109 void CostmapLayer::updateWithMax(
110 nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j,
int max_i,
int max_j )
{
if ( !enabled_ ) {
return;
}
unsigned char * master_array = master_grid.getCharMap( );
unsigned int span = master_grid.getSizeInCellsX( );
for ( int j = min_j; j < max_j; j++ ) {
unsigned int it = j * span + min_i;
for ( int i = min_i; i < max_i; i++ ) {
if ( costmap_[it] == NO_INFORMATION ) {
it++;
continue;
}
unsigned char old_cost = master_array[it];
if ( old_cost == NO_INFORMATION || old_cost < costmap_[it] ) {
master_array[it] = costmap_[it];
}
it++;
}
}
}
138 void CostmapLayer::updateWithTrueOverwrite(
139 nav2_costmap_2d::Costmap2D & master_grid, int min_i,
int min_j,
int max_i,
int max_j )
{
if ( !enabled_ ) {
return;
}
if ( costmap_ == nullptr ) {
throw std::runtime_error( "Can't update costmap layer: It has't been initialized yet!" );
}
unsigned char * master = master_grid.getCharMap( );
unsigned int span = master_grid.getSizeInCellsX( );
for ( int j = min_j; j < max_j; j++ ) {
unsigned int it = span * j + min_i;
for ( int i = min_i; i < max_i; i++ ) {
master[it] = costmap_[it];
it++;
}
}
}
164 void CostmapLayer::updateWithOverwrite(
165 nav2_costmap_2d::Costmap2D & master_grid,
int min_i, int min_j, int max_i, int max_j )
{
if ( !enabled_ ) {
return;
}
unsigned char * master = master_grid.getCharMap( );
unsigned int span = master_grid.getSizeInCellsX( );
for ( int j = min_j; j < max_j; j++ ) {
unsigned int it = span * j + min_i;
for ( int i = min_i; i < max_i; i++ ) {
if ( costmap_[it] != NO_INFORMATION ) {
master[it] = costmap_[it];
}
it++;
}
}
}
185 void CostmapLayer::updateWithAddition(
186 nav2_costmap_2d::Costmap2D & master_grid,
int min_i, int min_j, int max_i, int max_j )
{
if ( !enabled_ ) {
return;
}
unsigned char * master_array = master_grid.getCharMap( );
unsigned int span = master_grid.getSizeInCellsX( );
for ( int j = min_j; j < max_j; j++ ) {
unsigned int it = j * span + min_i;
for ( int i = min_i; i < max_i; i++ ) {
if ( costmap_[it] == NO_INFORMATION ) {
it++;
continue;
}
unsigned char old_cost = master_array[it];
if ( old_cost == NO_INFORMATION ) {
master_array[it] = costmap_[it];
} else {
int sum = old_cost + costmap_[it];
if ( sum >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE ) {
master_array[it] = nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1;
} else {
master_array[it] = sum;
}
}
it++;
}
}
}
} // namespace nav2_costmap_2d
1 /*
* Copyright ( c ) 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES ( INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE )
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <nav2_costmap_2d/costmap_math.hpp>
#include <vector>
34 double distanceToLine( double pX, double pY, double x0, double y0, double x1, double y1 )
{
double A = pX - x0;
double B = pY - y0;
double C = x1 - x0;
double D = y1 - y0;
double dot = A * C + B * D;
double len_sq = C * C + D * D;
double param = dot / len_sq;
double xx, yy;
if ( param < 0 ) {
xx = x0;
yy = y0;
} else if ( param > 1 ) {
xx = x1;
yy = y1;
} else {
xx = x0 + param * C;
yy = y0 + param * D;
}
return distance( pX, pY, xx, yy );
}
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <memory>
#include "nav2_costmap_2d/costmap_subscriber.hpp"
namespace nav2_costmap_2d
{
23 CostmapSubscriber::CostmapSubscriber(
24 const nav2_util::LifecycleNode::WeakPtr & parent,
25 const std::string & topic_name )
: topic_name_( topic_name )
{
auto node = parent.lock( );
costmap_sub_ = node->create_subscription<nav2_msgs::msg::Costmap>(
topic_name_,
rclcpp::QoS( rclcpp::KeepLast( 1 ) ).transient_local( ).reliable( ),
std::bind( &CostmapSubscriber::costmapCallback, this, std::placeholders::_1 ) );
}
35 CostmapSubscriber::CostmapSubscriber(
36 const rclcpp::Node::WeakPtr & parent,
37 const std::string & topic_name )
: topic_name_( topic_name )
{
auto node = parent.lock( );
costmap_sub_ = node->create_subscription<nav2_msgs::msg::Costmap>(
topic_name_,
rclcpp::QoS( rclcpp::KeepLast( 1 ) ).transient_local( ).reliable( ),
std::bind( &CostmapSubscriber::costmapCallback, this, std::placeholders::_1 ) );
}
47 std::shared_ptr<Costmap2D> CostmapSubscriber::getCostmap( )
{
if ( !costmap_received_ ) {
throw std::runtime_error( "Costmap is not available" );
}
toCostmap2D( );
return costmap_;
}
56 void CostmapSubscriber::toCostmap2D( )
{
auto current_costmap_msg = std::atomic_load( &costmap_msg_ );
if ( costmap_ == nullptr ) {
costmap_ = std::make_shared<Costmap2D>(
current_costmap_msg->metadata.size_x, current_costmap_msg->metadata.size_y,
current_costmap_msg->metadata.resolution, current_costmap_msg->metadata.origin.position.x,
current_costmap_msg->metadata.origin.position.y );
} else if ( costmap_->getSizeInCellsX( ) != current_costmap_msg->metadata.size_x || // NOLINT
costmap_->getSizeInCellsY( ) != current_costmap_msg->metadata.size_y ||
costmap_->getResolution( ) != current_costmap_msg->metadata.resolution ||
costmap_->getOriginX( ) != current_costmap_msg->metadata.origin.position.x ||
costmap_->getOriginY( ) != current_costmap_msg->metadata.origin.position.y )
{
// Update the size of the costmap
costmap_->resizeMap(
current_costmap_msg->metadata.size_x, current_costmap_msg->metadata.size_y,
current_costmap_msg->metadata.resolution,
current_costmap_msg->metadata.origin.position.x,
current_costmap_msg->metadata.origin.position.y );
}
unsigned char * master_array = costmap_->getCharMap( );
unsigned int index = 0;
for ( unsigned int i = 0; i < current_costmap_msg->metadata.size_x; ++i ) {
for ( unsigned int j = 0; j < current_costmap_msg->metadata.size_y; ++j ) {
master_array[index] = current_costmap_msg->data[index];
++index;
}
}
}
89 void CostmapSubscriber::costmapCallback( const nav2_msgs::msg::Costmap::SharedPtr msg )
{
std::atomic_store( &costmap_msg_, msg );
if ( !costmap_received_ ) {
costmap_received_ = true;
}
}
} // namespace nav2_costmap_2d
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Modified by: Shivang Patel ( shivaan14@gmail.com )
#include <memory>
#include <string>
#include <vector>
#include <algorithm>
#include <iostream>
#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
#include "nav2_costmap_2d/cost_values.hpp"
#include "nav2_costmap_2d/exceptions.hpp"
#include "nav2_costmap_2d/footprint.hpp"
#include "nav2_util/line_iterator.hpp"
using namespace std::chrono_literals;
namespace nav2_costmap_2d
{
35 CostmapTopicCollisionChecker::CostmapTopicCollisionChecker(
36 CostmapSubscriber & costmap_sub,
37 FootprintSubscriber & footprint_sub,
38 std::string name )
: name_( name ),
costmap_sub_( costmap_sub ),
footprint_sub_( footprint_sub ),
collision_checker_( nullptr )
{}
45 bool CostmapTopicCollisionChecker::isCollisionFree(
46 const geometry_msgs::msg::Pose2D & pose,
47 bool fetch_costmap_and_footprint )
{
try {
if ( scorePose( pose, fetch_costmap_and_footprint ) >= LETHAL_OBSTACLE ) {
return false;
}
return true;
} catch ( const IllegalPoseException & e ) {
RCLCPP_ERROR( rclcpp::get_logger( name_ ), "%s", e.what( ) );
return false;
} catch ( const CollisionCheckerException & e ) {
RCLCPP_ERROR( rclcpp::get_logger( name_ ), "%s", e.what( ) );
return false;
} catch ( ... ) {
RCLCPP_ERROR( rclcpp::get_logger( name_ ), "Failed to check pose score!" );
return false;
}
}
66 double CostmapTopicCollisionChecker::scorePose(
67 const geometry_msgs::msg::Pose2D & pose,
68 bool fetch_costmap_and_footprint )
{
if ( fetch_costmap_and_footprint ) {
try {
collision_checker_.setCostmap( costmap_sub_.getCostmap( ) );
} catch ( const std::runtime_error & e ) {
throw CollisionCheckerException( e.what( ) );
}
}
unsigned int cell_x, cell_y;
if ( !collision_checker_.worldToMap( pose.x, pose.y, cell_x, cell_y ) ) {
RCLCPP_DEBUG( rclcpp::get_logger( name_ ), "Map Cell: [%d, %d]", cell_x, cell_y );
throw IllegalPoseException( name_, "Pose Goes Off Grid." );
}
return collision_checker_.footprintCost( getFootprint( pose, fetch_costmap_and_footprint ) );
}
87 Footprint CostmapTopicCollisionChecker::getFootprint(
88 const geometry_msgs::msg::Pose2D & pose,
89 bool fetch_latest_footprint )
{
if ( fetch_latest_footprint ) {
std_msgs::msg::Header header;
if ( !footprint_sub_.getFootprintInRobotFrame( footprint_, header ) ) {
throw CollisionCheckerException( "Current footprint not available." );
}
}
Footprint footprint;
transformFootprint( pose.x, pose.y, pose.theta, footprint_, footprint );
return footprint;
}
} // namespace nav2_costmap_2d
1 /*
* Copyright ( c ) 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES ( INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE )
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "nav2_costmap_2d/footprint.hpp"
#include <algorithm>
#include <limits>
#include <string>
#include <vector>
#include "geometry_msgs/msg/point32.hpp"
#include "nav2_costmap_2d/array_parser.hpp"
#include "nav2_costmap_2d/costmap_math.hpp"
namespace nav2_costmap_2d
{
43 void calculateMinAndMaxDistances(
const std::vector<geometry_msgs::msg::Point> & footprint,
double & min_dist, double & max_dist )
{
min_dist = std::numeric_limits<double>::max( );
max_dist = 0.0;
if ( footprint.size( ) <= 2 ) {
return;
}
for ( unsigned int i = 0; i < footprint.size( ) - 1; ++i ) {
// check the distance from the robot center point to the first vertex
double vertex_dist = distance( 0.0, 0.0, footprint[i].x, footprint[i].y );
double edge_dist = distanceToLine(
0.0, 0.0, footprint[i].x, footprint[i].y,
footprint[i + 1].x, footprint[i + 1].y );
min_dist = std::min( min_dist, std::min( vertex_dist, edge_dist ) );
max_dist = std::max( max_dist, std::max( vertex_dist, edge_dist ) );
}
// we also need to do the last vertex and the first vertex
double vertex_dist = distance( 0.0, 0.0, footprint.back( ).x, footprint.back( ).y );
double edge_dist = distanceToLine(
0.0, 0.0, footprint.back( ).x, footprint.back( ).y,
footprint.front( ).x, footprint.front( ).y );
min_dist = std::min( min_dist, std::min( vertex_dist, edge_dist ) );
max_dist = std::max( max_dist, std::max( vertex_dist, edge_dist ) );
}
73 geometry_msgs::msg::Point32 toPoint32( geometry_msgs::msg::Point pt )
{
geometry_msgs::msg::Point32 point32;
point32.x = pt.x;
point32.y = pt.y;
point32.z = pt.z;
return point32;
}
82 geometry_msgs::msg::Point toPoint( geometry_msgs::msg::Point32 pt )
{
geometry_msgs::msg::Point point;
point.x = pt.x;
point.y = pt.y;
point.z = pt.z;
return point;
}
91 geometry_msgs::msg::Polygon toPolygon( std::vector<geometry_msgs::msg::Point> pts )
{
geometry_msgs::msg::Polygon polygon;
for ( unsigned int i = 0; i < pts.size( ); i++ ) {
polygon.points.push_back( toPoint32( pts[i] ) );
}
return polygon;
}
100 std::vector<geometry_msgs::msg::Point> toPointVector( geometry_msgs::msg::Polygon::SharedPtr polygon )
{
std::vector<geometry_msgs::msg::Point> pts;
for ( unsigned int i = 0; i < polygon->points.size( ); i++ ) {
pts.push_back( toPoint( polygon->points[i] ) );
}
return pts;
}
109 void transformFootprint(
double x, double y, double theta,
const std::vector<geometry_msgs::msg::Point> & footprint_spec,
std::vector<geometry_msgs::msg::Point> & oriented_footprint )
{
// build the oriented footprint at a given location
oriented_footprint.resize( footprint_spec.size( ) );
double cos_th = cos( theta );
double sin_th = sin( theta );
for ( unsigned int i = 0; i < footprint_spec.size( ); ++i ) {
double new_x = x + ( footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th );
double new_y = y + ( footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th );
geometry_msgs::msg::Point & new_pt = oriented_footprint[i];
new_pt.x = new_x;
new_pt.y = new_y;
}
}
127 void transformFootprint(
double x, double y, double theta,
const std::vector<geometry_msgs::msg::Point> & footprint_spec,
geometry_msgs::msg::PolygonStamped & oriented_footprint )
{
// build the oriented footprint at a given location
oriented_footprint.polygon.points.clear( );
double cos_th = cos( theta );
double sin_th = sin( theta );
for ( unsigned int i = 0; i < footprint_spec.size( ); ++i ) {
geometry_msgs::msg::Point32 new_pt;
new_pt.x = x + ( footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th );
new_pt.y = y + ( footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th );
oriented_footprint.polygon.points.push_back( new_pt );
}
}
144 void padFootprint( std::vector<geometry_msgs::msg::Point> & footprint, double padding )
{
// pad footprint in place
for ( unsigned int i = 0; i < footprint.size( ); i++ ) {
geometry_msgs::msg::Point & pt = footprint[i];
pt.x += sign0( pt.x ) * padding;
pt.y += sign0( pt.y ) * padding;
}
}
155 std::vector<geometry_msgs::msg::Point> makeFootprintFromRadius( double radius )
{
std::vector<geometry_msgs::msg::Point> points;
// Loop over 16 angles around a circle making a point each time
int N = 16;
geometry_msgs::msg::Point pt;
for ( int i = 0; i < N; ++i ) {
double angle = i * 2 * M_PI / N;
pt.x = cos( angle ) * radius;
pt.y = sin( angle ) * radius;
points.push_back( pt );
}
return points;
}
174 bool makeFootprintFromString(
const std::string & footprint_string,
std::vector<geometry_msgs::msg::Point> & footprint )
{
std::string error;
std::vector<std::vector<float>> vvf = parseVVF( footprint_string, error );
if ( error != "" ) {
RCLCPP_ERROR(
rclcpp::get_logger(
"nav2_costmap_2d" ), "Error parsing footprint parameter: '%s'", error.c_str( ) );
RCLCPP_ERROR(
rclcpp::get_logger(
"nav2_costmap_2d" ), " Footprint string was '%s'.", footprint_string.c_str( ) );
return false;
}
// convert vvf into points.
if ( vvf.size( ) < 3 ) {
RCLCPP_ERROR(
rclcpp::get_logger(
"nav2_costmap_2d" ),
"You must specify at least three points for the robot footprint, reverting to previous footprint." ); //NOLINT
return false;
}
footprint.reserve( vvf.size( ) );
for ( unsigned int i = 0; i < vvf.size( ); i++ ) {
if ( vvf[i].size( ) == 2 ) {
geometry_msgs::msg::Point point;
point.x = vvf[i][0];
point.y = vvf[i][1];
point.z = 0;
footprint.push_back( point );
} else {
RCLCPP_ERROR(
rclcpp::get_logger(
"nav2_costmap_2d" ),
"Points in the footprint specification must be pairs of numbers. Found a point with %d numbers.", //NOLINT
static_cast<int>( vvf[i].size( ) ) );
return false;
}
}
return true;
}
} // end namespace nav2_costmap_2d
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Modified by: Shivang Patel ( shivaang14@gmail.com )
#include <memory>
#include <string>
#include <vector>
#include <algorithm>
#include "nav2_costmap_2d/footprint_collision_checker.hpp"
#include "nav2_costmap_2d/cost_values.hpp"
#include "nav2_costmap_2d/exceptions.hpp"
#include "nav2_costmap_2d/footprint.hpp"
#include "nav2_util/line_iterator.hpp"
using namespace std::chrono_literals;
namespace nav2_costmap_2d
{
template<typename CostmapT>
35 FootprintCollisionChecker<CostmapT>::FootprintCollisionChecker( )
: costmap_( nullptr )
{
}
template<typename CostmapT>
41 FootprintCollisionChecker<CostmapT>::FootprintCollisionChecker(
42 CostmapT costmap )
: costmap_( costmap )
{
}
template<typename CostmapT>
48 double FootprintCollisionChecker<CostmapT>::footprintCost( const Footprint footprint )
{
// now we really have to lay down the footprint in the costmap_ grid
unsigned int x0, x1, y0, y1;
double footprint_cost = 0.0;
// get the cell coord of the first point
if ( !worldToMap( footprint[0].x, footprint[0].y, x0, y0 ) ) {
return static_cast<double>( LETHAL_OBSTACLE );
}
// cache the start to eliminate a worldToMap call
unsigned int xstart = x0;
unsigned int ystart = y0;
// we need to rasterize each line in the footprint
for ( unsigned int i = 0; i < footprint.size( ) - 1; ++i ) {
// get the cell coord of the second point
if ( !worldToMap( footprint[i + 1].x, footprint[i + 1].y, x1, y1 ) ) {
return static_cast<double>( LETHAL_OBSTACLE );
}
footprint_cost = std::max( lineCost( x0, x1, y0, y1 ), footprint_cost );
// the second point is next iteration's first point
x0 = x1;
y0 = y1;
// if in collision, no need to continue
if ( footprint_cost == static_cast<double>( LETHAL_OBSTACLE ) ) {
return footprint_cost;
}
}
// we also need to connect the first point in the footprint to the last point
// the last iteration's x1, y1 are the last footprint point's coordinates
return std::max( lineCost( xstart, x1, ystart, y1 ), footprint_cost );
}
template<typename CostmapT>
88 double FootprintCollisionChecker<CostmapT>::lineCost( int x0, int x1, int y0, int y1 ) const
{
double line_cost = 0.0;
double point_cost = -1.0;
for ( nav2_util::LineIterator line( x0, y0, x1, y1 ); line.isValid( ); line.advance( ) ) {
point_cost = pointCost( line.getX( ), line.getY( ) ); // Score the current point
// if in collision, no need to continue
if ( point_cost == static_cast<double>( LETHAL_OBSTACLE ) ) {
return point_cost;
}
if ( line_cost < point_cost ) {
line_cost = point_cost;
}
}
return line_cost;
}
template<typename CostmapT>
110 bool FootprintCollisionChecker<CostmapT>::worldToMap(
double wx, double wy, unsigned int & mx, unsigned int & my )
{
return costmap_->worldToMap( wx, wy, mx, my );
}
template<typename CostmapT>
117 double FootprintCollisionChecker<CostmapT>::pointCost( int x, int y ) const
{
return costmap_->getCost( x, y );
}
template<typename CostmapT>
123 void FootprintCollisionChecker<CostmapT>::setCostmap( CostmapT costmap )
{
costmap_ = costmap;
}
template<typename CostmapT>
129 double FootprintCollisionChecker<CostmapT>::footprintCostAtPose(
130 double x, double y, double theta, const Footprint footprint )
{
double cos_th = cos( theta );
double sin_th = sin( theta );
Footprint oriented_footprint;
for ( unsigned int i = 0; i < footprint.size( ); ++i ) {
geometry_msgs::msg::Point new_pt;
new_pt.x = x + ( footprint[i].x * cos_th - footprint[i].y * sin_th );
new_pt.y = y + ( footprint[i].x * sin_th + footprint[i].y * cos_th );
oriented_footprint.push_back( new_pt );
}
return footprintCost( oriented_footprint );
}
// declare our valid template parameters
146 template class FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>;
template class FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>;
} // namespace nav2_costmap_2d
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <vector>
#include <memory>
#include "nav2_costmap_2d/footprint_subscriber.hpp"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpedantic"
#include "tf2/utils.h"
#pragma GCC diagnostic pop
namespace nav2_costmap_2d
{
28 FootprintSubscriber::FootprintSubscriber(
29 const nav2_util::LifecycleNode::WeakPtr & parent,
30 const std::string & topic_name,
31 tf2_ros::Buffer & tf,
32 std::string robot_base_frame,
double transform_tolerance )
: tf_( tf ),
robot_base_frame_( robot_base_frame ),
transform_tolerance_( transform_tolerance )
{
auto node = parent.lock( );
footprint_sub_ = node->create_subscription<geometry_msgs::msg::PolygonStamped>(
topic_name, rclcpp::SystemDefaultsQoS( ),
std::bind( &FootprintSubscriber::footprint_callback, this, std::placeholders::_1 ) );
}
44 FootprintSubscriber::FootprintSubscriber(
45 const rclcpp::Node::WeakPtr & parent,
46 const std::string & topic_name,
47 tf2_ros::Buffer & tf,
48 std::string robot_base_frame,
double transform_tolerance )
: tf_( tf ),
robot_base_frame_( robot_base_frame ),
transform_tolerance_( transform_tolerance )
{
auto node = parent.lock( );
footprint_sub_ = node->create_subscription<geometry_msgs::msg::PolygonStamped>(
topic_name, rclcpp::SystemDefaultsQoS( ),
std::bind( &FootprintSubscriber::footprint_callback, this, std::placeholders::_1 ) );
}
bool
61 FootprintSubscriber::getFootprintRaw(
62 std::vector<geometry_msgs::msg::Point> & footprint,
63 std_msgs::msg::Header & footprint_header )
{
if ( !footprint_received_ ) {
return false;
}
auto current_footprint = std::atomic_load( &footprint_ );
footprint = toPointVector(
std::make_shared<geometry_msgs::msg::Polygon>( current_footprint->polygon ) );
footprint_header = current_footprint->header;
return true;
}
bool
78 FootprintSubscriber::getFootprintInRobotFrame(
79 std::vector<geometry_msgs::msg::Point> & footprint,
80 std_msgs::msg::Header & footprint_header )
{
if ( !getFootprintRaw( footprint, footprint_header ) ) {
return false;
}
geometry_msgs::msg::PoseStamped current_pose;
if ( !nav2_util::getCurrentPose(
current_pose, tf_, footprint_header.frame_id, robot_base_frame_,
transform_tolerance_, footprint_header.stamp ) )
{
return false;
}
double x = current_pose.pose.position.x;
double y = current_pose.pose.position.y;
double theta = tf2::getYaw( current_pose.pose.orientation );
std::vector<geometry_msgs::msg::Point> temp;
transformFootprint( -x, -y, 0, footprint, temp );
transformFootprint( 0, 0, -theta, temp, footprint );
footprint_header.frame_id = robot_base_frame_;
footprint_header.stamp = current_pose.header.stamp;
return true;
}
void
109 FootprintSubscriber::footprint_callback( const geometry_msgs::msg::PolygonStamped::SharedPtr msg )
{
std::atomic_store( &footprint_, msg );
if ( !footprint_received_ ) {
footprint_received_ = true;
}
}
} // namespace nav2_costmap_2d
1 /*
* Copyright ( c ) 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES ( INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE )
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "nav2_costmap_2d/layer.hpp"
#include <string>
#include <vector>
#include "nav2_util/node_utils.hpp"
namespace nav2_costmap_2d
{
39 Layer::Layer( )
: layered_costmap_( nullptr ),
name_( ),
tf_( nullptr ),
current_( false ),
enabled_( false )
{}
void
48 Layer::initialize(
49 LayeredCostmap * parent,
50 std::string name,
51 tf2_ros::Buffer * tf,
52 const nav2_util::LifecycleNode::WeakPtr & node,
53 rclcpp::CallbackGroup::SharedPtr callback_group )
{
layered_costmap_ = parent;
name_ = name;
tf_ = tf;
node_ = node;
callback_group_ = callback_group;
{
auto node_shared_ptr = node_.lock( );
logger_ = node_shared_ptr->get_logger( );
clock_ = node_shared_ptr->get_clock( );
}
onInitialize( );
}
const std::vector<geometry_msgs::msg::Point> &
70 Layer::getFootprint( ) const
{
return layered_costmap_->getFootprint( );
}
void
76 Layer::declareParameter(
77 const std::string & param_name,
78 const rclcpp::ParameterValue & value )
{
auto node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
local_params_.insert( param_name );
nav2_util::declare_parameter_if_not_declared(
node, getFullName( param_name ), value );
}
void
90 Layer::declareParameter(
91 const std::string & param_name,
92 const rclcpp::ParameterType & param_type )
{
auto node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
local_params_.insert( param_name );
nav2_util::declare_parameter_if_not_declared(
node, getFullName( param_name ), param_type );
}
bool
104 Layer::hasParameter( const std::string & param_name )
{
auto node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
return node->has_parameter( getFullName( param_name ) );
}
std::string
114 Layer::getFullName( const std::string & param_name )
{
return std::string( name_ + "." + param_name );
}
} // end namespace nav2_costmap_2d
1 /*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include "nav2_costmap_2d/layered_costmap.hpp"
#include <algorithm>
#include <cstdio>
#include <memory>
#include <string>
#include <vector>
#include <limits>
#include "nav2_costmap_2d/footprint.hpp"
using std::vector;
namespace nav2_costmap_2d
{
55 LayeredCostmap::LayeredCostmap( std::string global_frame, bool rolling_window, bool track_unknown )
: primary_costmap_( ), combined_costmap_( ),
global_frame_( global_frame ),
rolling_window_( rolling_window ),
current_( false ),
minx_( 0.0 ),
miny_( 0.0 ),
maxx_( 0.0 ),
maxy_( 0.0 ),
bx0_( 0 ),
bxn_( 0 ),
by0_( 0 ),
byn_( 0 ),
initialized_( false ),
size_locked_( false ),
circumscribed_radius_( 1.0 ),
inscribed_radius_( 0.1 )
{
if ( track_unknown ) {
primary_costmap_.setDefaultValue( 255 );
combined_costmap_.setDefaultValue( 255 );
} else {
primary_costmap_.setDefaultValue( 0 );
combined_costmap_.setDefaultValue( 0 );
}
}
82 LayeredCostmap::~LayeredCostmap( )
{
while ( plugins_.size( ) > 0 ) {
plugins_.pop_back( );
}
while ( filters_.size( ) > 0 ) {
filters_.pop_back( );
}
}
92 void LayeredCostmap::addPlugin( std::shared_ptr<Layer> plugin )
{
std::unique_lock<Costmap2D::mutex_t> lock( *( combined_costmap_.getMutex( ) ) );
plugins_.push_back( plugin );
}
98 void LayeredCostmap::resizeMap(
unsigned int size_x, unsigned int size_y, double resolution,
double origin_x,
double origin_y,
102 bool size_locked )
{
std::unique_lock<Costmap2D::mutex_t> lock( *( combined_costmap_.getMutex( ) ) );
size_locked_ = size_locked;
primary_costmap_.resizeMap( size_x, size_y, resolution, origin_x, origin_y );
combined_costmap_.resizeMap( size_x, size_y, resolution, origin_x, origin_y );
for ( vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin( );
plugin != plugins_.end( ); ++plugin )
{
( *plugin )->matchSize( );
}
for ( vector<std::shared_ptr<Layer>>::iterator filter = filters_.begin( );
filter != filters_.end( ); ++filter )
{
( *filter )->matchSize( );
}
}
120 bool LayeredCostmap::isOutofBounds( double robot_x, double robot_y )
{
unsigned int mx, my;
return !combined_costmap_.worldToMap( robot_x, robot_y, mx, my );
}
126 void LayeredCostmap::updateMap( double robot_x, double robot_y, double robot_yaw )
{
// Lock for the remainder of this function, some plugins ( e.g. VoxelLayer )
// implement thread unsafe updateBounds( ) functions.
std::unique_lock<Costmap2D::mutex_t> lock( *( combined_costmap_.getMutex( ) ) );
// if we're using a rolling buffer costmap...
// we need to update the origin using the robot's position
if ( rolling_window_ ) {
double new_origin_x = robot_x - combined_costmap_.getSizeInMetersX( ) / 2;
double new_origin_y = robot_y - combined_costmap_.getSizeInMetersY( ) / 2;
primary_costmap_.updateOrigin( new_origin_x, new_origin_y );
combined_costmap_.updateOrigin( new_origin_x, new_origin_y );
}
if ( isOutofBounds( robot_x, robot_y ) ) {
RCLCPP_WARN(
rclcpp::get_logger( "nav2_costmap_2d" ),
"Robot is out of bounds of the costmap!" );
}
if ( plugins_.size( ) == 0 && filters_.size( ) == 0 ) {
return;
}
minx_ = miny_ = std::numeric_limits<double>::max( );
maxx_ = maxy_ = std::numeric_limits<double>::lowest( );
for ( vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin( );
plugin != plugins_.end( ); ++plugin )
{
double prev_minx = minx_;
double prev_miny = miny_;
double prev_maxx = maxx_;
double prev_maxy = maxy_;
( *plugin )->updateBounds( robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_ );
if ( minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy ) {
RCLCPP_WARN(
rclcpp::get_logger(
"nav2_costmap_2d" ), "Illegal bounds change, was [tl: ( %f, %f ), br: ( %f, %f )], but "
"is now [tl: ( %f, %f ), br: ( %f, %f )]. The offending layer is %s",
prev_minx, prev_miny, prev_maxx, prev_maxy,
minx_, miny_, maxx_, maxy_,
( *plugin )->getName( ).c_str( ) );
}
}
for ( vector<std::shared_ptr<Layer>>::iterator filter = filters_.begin( );
filter != filters_.end( ); ++filter )
{
double prev_minx = minx_;
double prev_miny = miny_;
double prev_maxx = maxx_;
double prev_maxy = maxy_;
( *filter )->updateBounds( robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_ );
if ( minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy ) {
RCLCPP_WARN(
rclcpp::get_logger(
"nav2_costmap_2d" ), "Illegal bounds change, was [tl: ( %f, %f ), br: ( %f, %f )], but "
"is now [tl: ( %f, %f ), br: ( %f, %f )]. The offending filter is %s",
prev_minx, prev_miny, prev_maxx, prev_maxy,
minx_, miny_, maxx_, maxy_,
( *filter )->getName( ).c_str( ) );
}
}
int x0, xn, y0, yn;
combined_costmap_.worldToMapEnforceBounds( minx_, miny_, x0, y0 );
combined_costmap_.worldToMapEnforceBounds( maxx_, maxy_, xn, yn );
x0 = std::max( 0, x0 );
xn = std::min( static_cast<int>( combined_costmap_.getSizeInCellsX( ) ), xn + 1 );
y0 = std::max( 0, y0 );
yn = std::min( static_cast<int>( combined_costmap_.getSizeInCellsY( ) ), yn + 1 );
RCLCPP_DEBUG(
rclcpp::get_logger(
"nav2_costmap_2d" ), "Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn );
if ( xn < x0 || yn < y0 ) {
return;
}
if ( filters_.size( ) == 0 ) {
// If there are no filters enabled just update costmap sequentially by each plugin
combined_costmap_.resetMap( x0, y0, xn, yn );
for ( vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin( );
plugin != plugins_.end( ); ++plugin )
{
( *plugin )->updateCosts( combined_costmap_, x0, y0, xn, yn );
}
} else {
// Costmap Filters enabled
// 1. Update costmap by plugins
primary_costmap_.resetMap( x0, y0, xn, yn );
for ( vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin( );
plugin != plugins_.end( ); ++plugin )
{
( *plugin )->updateCosts( primary_costmap_, x0, y0, xn, yn );
}
// 2. Copy processed costmap window to a final costmap.
// primary_costmap_ remain to be untouched for further usage by plugins.
if ( !combined_costmap_.copyWindow( primary_costmap_, x0, y0, xn, yn, x0, y0 ) ) {
RCLCPP_ERROR(
rclcpp::get_logger( "nav2_costmap_2d" ),
"Can not copy costmap ( %i, %i )..( %i, %i ) window",
x0, y0, xn, yn );
throw std::runtime_error{"Can not copy costmap"};
}
// 3. Apply filters over the plugins in order to make filters' work
// not being considered by plugins on next updateMap( ) calls
for ( vector<std::shared_ptr<Layer>>::iterator filter = filters_.begin( );
filter != filters_.end( ); ++filter )
{
( *filter )->updateCosts( combined_costmap_, x0, y0, xn, yn );
}
}
bx0_ = x0;
bxn_ = xn;
by0_ = y0;
byn_ = yn;
initialized_ = true;
}
253 bool LayeredCostmap::isCurrent( )
{
current_ = true;
for ( vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin( );
plugin != plugins_.end( ); ++plugin )
{
current_ = current_ && ( *plugin )->isCurrent( );
}
for ( vector<std::shared_ptr<Layer>>::iterator filter = filters_.begin( );
filter != filters_.end( ); ++filter )
{
current_ = current_ && ( *filter )->isCurrent( );
}
return current_;
}
269 void LayeredCostmap::setFootprint( const std::vector<geometry_msgs::msg::Point> & footprint_spec )
{
footprint_ = footprint_spec;
nav2_costmap_2d::calculateMinAndMaxDistances(
footprint_spec,
inscribed_radius_, circumscribed_radius_ );
for ( vector<std::shared_ptr<Layer>>::iterator plugin = plugins_.begin( );
plugin != plugins_.end( );
++plugin )
{
( *plugin )->onFootprintChanged( );
}
for ( vector<std::shared_ptr<Layer>>::iterator filter = filters_.begin( );
filter != filters_.end( );
++filter )
{
( *filter )->onFootprintChanged( );
}
}
} // namespace nav2_costmap_2d
1 /*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#include "nav2_costmap_2d/observation_buffer.hpp"
#include <algorithm>
#include <list>
#include <string>
#include <vector>
#include <chrono>
#include "tf2/convert.h"
#include "sensor_msgs/point_cloud2_iterator.hpp"
using namespace std::chrono_literals;
namespace nav2_costmap_2d
{
51 ObservationBuffer::ObservationBuffer(
52 const nav2_util::LifecycleNode::WeakPtr & parent,
53 std::string topic_name,
double observation_keep_time,
double expected_update_rate,
double min_obstacle_height, double max_obstacle_height, double obstacle_max_range,
double obstacle_min_range,
58 double raytrace_max_range, double raytrace_min_range, tf2_ros::Buffer & tf2_buffer,
59 std::string global_frame,
60 std::string sensor_frame,
61 tf2::Duration tf_tolerance )
: tf2_buffer_( tf2_buffer ),
observation_keep_time_( rclcpp::Duration::from_seconds( observation_keep_time ) ),
expected_update_rate_( rclcpp::Duration::from_seconds( expected_update_rate ) ),
global_frame_( global_frame ),
sensor_frame_( sensor_frame ),
topic_name_( topic_name ),
min_obstacle_height_( min_obstacle_height ), max_obstacle_height_( max_obstacle_height ),
obstacle_max_range_( obstacle_max_range ), obstacle_min_range_( obstacle_min_range ),
raytrace_max_range_( raytrace_max_range ), raytrace_min_range_(
raytrace_min_range ), tf_tolerance_( tf_tolerance )
{
auto node = parent.lock( );
clock_ = node->get_clock( );
logger_ = node->get_logger( );
last_updated_ = node->now( );
}
79 ObservationBuffer::~ObservationBuffer( )
{
}
83 void ObservationBuffer::bufferCloud( const sensor_msgs::msg::PointCloud2 & cloud )
{
geometry_msgs::msg::PointStamped global_origin;
// create a new observation on the list to be populated
observation_list_.push_front( Observation( ) );
// check whether the origin frame has been set explicitly
// or whether we should get it from the cloud
std::string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;
try {
// given these observations come from sensors...
// we'll need to store the origin pt of the sensor
geometry_msgs::msg::PointStamped local_origin;
local_origin.header.stamp = cloud.header.stamp;
local_origin.header.frame_id = origin_frame;
local_origin.point.x = 0;
local_origin.point.y = 0;
local_origin.point.z = 0;
tf2_buffer_.transform( local_origin, global_origin, global_frame_, tf_tolerance_ );
tf2::convert( global_origin.point, observation_list_.front( ).origin_ );
// make sure to pass on the raytrace/obstacle range
// of the observation buffer to the observations
observation_list_.front( ).raytrace_max_range_ = raytrace_max_range_;
observation_list_.front( ).raytrace_min_range_ = raytrace_min_range_;
observation_list_.front( ).obstacle_max_range_ = obstacle_max_range_;
observation_list_.front( ).obstacle_min_range_ = obstacle_min_range_;
sensor_msgs::msg::PointCloud2 global_frame_cloud;
// transform the point cloud
tf2_buffer_.transform( cloud, global_frame_cloud, global_frame_, tf_tolerance_ );
global_frame_cloud.header.stamp = cloud.header.stamp;
// now we need to remove observations from the cloud that are below
// or above our height thresholds
sensor_msgs::msg::PointCloud2 & observation_cloud = *( observation_list_.front( ).cloud_ );
observation_cloud.height = global_frame_cloud.height;
observation_cloud.width = global_frame_cloud.width;
observation_cloud.fields = global_frame_cloud.fields;
observation_cloud.is_bigendian = global_frame_cloud.is_bigendian;
observation_cloud.point_step = global_frame_cloud.point_step;
observation_cloud.row_step = global_frame_cloud.row_step;
observation_cloud.is_dense = global_frame_cloud.is_dense;
unsigned int cloud_size = global_frame_cloud.height * global_frame_cloud.width;
sensor_msgs::PointCloud2Modifier modifier( observation_cloud );
modifier.resize( cloud_size );
unsigned int point_count = 0;
// copy over the points that are within our height bounds
sensor_msgs::PointCloud2Iterator<float> iter_z( global_frame_cloud, "z" );
std::vector<unsigned char>::const_iterator iter_global = global_frame_cloud.data.begin( ),
iter_global_end = global_frame_cloud.data.end( );
std::vector<unsigned char>::iterator iter_obs = observation_cloud.data.begin( );
for ( ; iter_global != iter_global_end; ++iter_z, iter_global +=
global_frame_cloud.point_step )
{
if ( ( *iter_z ) <= max_obstacle_height_ &&
( *iter_z ) >= min_obstacle_height_ )
{
std::copy( iter_global, iter_global + global_frame_cloud.point_step, iter_obs );
iter_obs += global_frame_cloud.point_step;
++point_count;
}
}
// resize the cloud for the number of legal points
modifier.resize( point_count );
observation_cloud.header.stamp = cloud.header.stamp;
observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;
} catch ( tf2::TransformException & ex ) {
// if an exception occurs, we need to remove the empty observation from the list
observation_list_.pop_front( );
RCLCPP_ERROR(
logger_,
"TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s",
sensor_frame_.c_str( ),
cloud.header.frame_id.c_str( ), ex.what( ) );
return;
}
// if the update was successful, we want to update the last updated time
last_updated_ = clock_->now( );
// we'll also remove any stale observations from the list
purgeStaleObservations( );
}
// returns a copy of the observations
175 void ObservationBuffer::getObservations( std::vector<Observation> & observations )
{
// first... let's make sure that we don't have any stale observations
purgeStaleObservations( );
// now we'll just copy the observations for the caller
std::list<Observation>::iterator obs_it;
for ( obs_it = observation_list_.begin( ); obs_it != observation_list_.end( ); ++obs_it ) {
observations.push_back( *obs_it );
}
}
187 void ObservationBuffer::purgeStaleObservations( )
{
if ( !observation_list_.empty( ) ) {
std::list<Observation>::iterator obs_it = observation_list_.begin( );
// if we're keeping observations for no time... then we'll only keep one observation
if ( observation_keep_time_ == rclcpp::Duration( 0.0s ) ) {
observation_list_.erase( ++obs_it, observation_list_.end( ) );
return;
}
// otherwise... we'll have to loop through the observations to see which ones are stale
for ( obs_it = observation_list_.begin( ); obs_it != observation_list_.end( ); ++obs_it ) {
Observation & obs = *obs_it;
// check if the observation is out of date... and if it is,
// remove it and those that follow from the list
if ( ( clock_->now( ) - obs.cloud_->header.stamp ) >
observation_keep_time_ )
{
observation_list_.erase( obs_it, observation_list_.end( ) );
return;
}
}
}
}
212 bool ObservationBuffer::isCurrent( ) const
{
if ( expected_update_rate_ == rclcpp::Duration( 0.0s ) ) {
return true;
}
bool current = ( clock_->now( ) - last_updated_ ) <=
expected_update_rate_;
if ( !current ) {
RCLCPP_WARN(
logger_,
"The %s observation buffer has not been updated for %.2f seconds, "
"and it should be updated every %.2f seconds.",
topic_name_.c_str( ),
( clock_->now( ) - last_updated_ ).seconds( ),
expected_update_rate_.seconds( ) );
}
return current;
}
232 void ObservationBuffer::resetLastUpdated( )
{
last_updated_ = clock_->now( );
}
} // namespace nav2_costmap_2d
1 /*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#include <memory>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_listener.h"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav2_costmap_2d/cost_values.hpp"
namespace nav2_costmap_2d
{
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
50 class CostmapTester : public testing::Test
{
public:
53 explicit CostmapTester( tf2_ros::Buffer & tf );
54 void checkConsistentCosts( );
55 void compareCellToNeighbors(
56 nav2_costmap_2d::Costmap2D & costmap,
unsigned int x, unsigned int y );
58 void compareCells(
59 nav2_costmap_2d::Costmap2D & costmap,
unsigned int x, unsigned int y, unsigned int nx, unsigned int ny );
61 virtual void TestBody( ) {}
};
64 CostmapTester::CostmapTester( tf2_ros::Buffer & tf )
{
costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "test_costmap", tf );
}
69 void CostmapTester::checkConsistentCosts( )
{
nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap( );
// get a copy of the costmap contained by our ros wrapper
costmap->saveMap( "costmap_test.pgm" );
// loop through the costmap and check for any unexpected drop-offs in costs
for ( unsigned int i = 0; i < costmap->getSizeInCellsX( ); ++i ) {
for ( unsigned int j = 0; j < costmap->getSizeInCellsY( ); ++j ) {
compareCellToNeighbors( *costmap, i, j );
}
}
}
84 void CostmapTester::compareCellToNeighbors(
85 nav2_costmap_2d::Costmap2D & costmap,
unsigned int x, unsigned int y )
{
// we'll compare the cost of this cell with that of
// its eight neighbors to see if they're reasonable
for ( int offset_x = -1; offset_x <= 1; ++offset_x ) {
for ( int offset_y = -1; offset_y <= 1; ++offset_y ) {
int nx = x + offset_x;
int ny = y + offset_y;
// check to make sure that the neighbor cell is a legal one
if ( nx >= 0 && nx < static_cast<int>( costmap.getSizeInCellsX( ) ) && ny >= 0 &&
ny < static_cast<int>( costmap.getSizeInCellsY( ) ) )
{
compareCells( costmap, x, y, nx, ny );
}
}
}
}
// for all lethal and inscribed costs,
// we'll make sure that their neighbors have the cost values we'd expect
107 void CostmapTester::compareCells(
108 nav2_costmap_2d::Costmap2D & costmap,
unsigned int x, unsigned int y, unsigned int nx, unsigned int ny )
{
double cell_distance = hypot( static_cast<int>( x - nx ), static_cast<int>( y - ny ) );
unsigned char cell_cost = costmap.getCost( x, y );
unsigned char neighbor_cost = costmap.getCost( nx, ny );
if ( cell_cost == nav2_costmap_2d::LETHAL_OBSTACLE ) {
// if the cell is a lethal obstacle,
// then we know that all its neighbors should have equal or slighlty less cost
unsigned char expected_lowest_cost = 0;
EXPECT_TRUE(
neighbor_cost >= expected_lowest_cost ||
( cell_distance > 0 && neighbor_cost == nav2_costmap_2d::FREE_SPACE ) );
} else if ( cell_cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE ) {
// the furthest valid distance from an obstacle
// is the inscribed radius plus the cell distance away
double furthest_valid_distance = 0;
unsigned char expected_lowest_cost = 0;
if ( neighbor_cost < expected_lowest_cost ) {
RCLCPP_ERROR(
rclcpp::get_logger(
"costmap_tester" ),
"Cell cost ( %d, %d ): %d, neighbor cost ( %d, %d ): %d, expected lowest cost: %d, cell distance: %.2f, furthest valid distance: %.2f", // NOLINT
x, y, cell_cost, nx, ny, neighbor_cost, expected_lowest_cost,
cell_distance, furthest_valid_distance );
RCLCPP_ERROR(
rclcpp::get_logger( "costmap_tester" ), "Cell: ( %d, %d ), Neighbor: ( %d, %d )",
x, y, nx, ny );
costmap.saveMap( "failing_costmap.pgm" );
}
EXPECT_TRUE(
neighbor_cost >= expected_lowest_cost ||
( furthest_valid_distance > 0 && neighbor_cost == nav2_costmap_2d::FREE_SPACE ) );
}
}
} // namespace nav2_costmap_2d
nav2_costmap_2d::CostmapTester * map_tester = NULL;
tf2_ros::TransformListener * tfl_;
tf2_ros::Buffer * tf_;
151 TEST( CostmapTester, checkConsistentCosts ) {
map_tester->checkConsistentCosts( );
}
155 void testCallback( )
{
int test_result = RUN_ALL_TESTS( );
RCLCPP_INFO( rclcpp::get_logger( "costmap_tester" ), "gtest return value: %d", test_result );
}
161 int main( int argc, char ** argv )
{
rclcpp::init( argc, argv );
auto node = nav2_util::LifecycleNode::make_shared( "costmap_tester" );
testing::InitGoogleTest( &argc, argv );
tf_ = new tf2_ros::Buffer( node->get_clock( ) );
tfl_ = new tf2_ros::TransformListener( *tf_ );
map_tester = new nav2_costmap_2d::CostmapTester( *tf_ );
rclcpp::TimerBase::SharedPtr timer = node->create_wall_timer( 30000ms, testCallback );
rclcpp::spin( costmap_ros_ );
rclcpp::shutdown( );
return 0;
}
1 // Copyright ( c ) 2021 Wyca Robotics
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include <vector>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "tf2_ros/transform_broadcaster.h"
24 class RclCppFixture
{
public:
27 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
28 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
32 class DynParamTestNode
{
public:
35 DynParamTestNode( ) {}
36 ~DynParamTestNode( ) {}
};
39 TEST( DynParamTestNode, testDynParamsSet )
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>( "dyn_param_tester" );
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "test_costmap" );
costmap->on_configure( rclcpp_lifecycle::State( ) );
// Set tf between default global_frame and robot_base_frame in order not to block in on_activate
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_ =
std::make_unique<tf2_ros::TransformBroadcaster>( node );
geometry_msgs::msg::TransformStamped t;
t.header.stamp = node->get_clock( )->now( );
t.header.frame_id = "map";
t.child_frame_id = "base_link";
tf_broadcaster_->sendTransform( t );
t.header.frame_id = "map";
t.child_frame_id = "test_frame";
tf_broadcaster_->sendTransform( t );
costmap->on_activate( rclcpp_lifecycle::State( ) );
auto parameter_client = std::make_shared<rclcpp::AsyncParametersClient>(
node->shared_from_this( ),
"/test_costmap/test_costmap",
rmw_qos_profile_parameters );
auto results1 = parameter_client->set_parameters_atomically(
{
rclcpp::Parameter( "robot_radius", 1.234 ),
rclcpp::Parameter( "footprint_padding", 2.345 ),
rclcpp::Parameter( "transform_tolerance", 3.456 ),
rclcpp::Parameter( "publish_frequency", 4.567 ),
rclcpp::Parameter( "resolution", 5.678 ),
rclcpp::Parameter( "origin_x", 6.789 ),
rclcpp::Parameter( "origin_y", 7.891 ),
rclcpp::Parameter( "width", 2 ),
rclcpp::Parameter( "height", 3 ),
rclcpp::Parameter(
"footprint",
"[[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]]" ),
rclcpp::Parameter( "robot_base_frame", "test_frame" ),
} );
// Try setting robot_base_frame to an invalid frame, should be rejected
auto results2 = parameter_client->set_parameters_atomically(
{
rclcpp::Parameter( "robot_base_frame", "wrong_test_frame" ),
} );
rclcpp::spin_some( costmap->get_node_base_interface( ) );
EXPECT_EQ( costmap->get_parameter( "robot_radius" ).as_double( ), 1.234 );
EXPECT_EQ( costmap->get_parameter( "footprint_padding" ).as_double( ), 2.345 );
EXPECT_EQ( costmap->get_parameter( "transform_tolerance" ).as_double( ), 3.456 );
EXPECT_EQ( costmap->get_parameter( "publish_frequency" ).as_double( ), 4.567 );
EXPECT_EQ( costmap->get_parameter( "resolution" ).as_double( ), 5.678 );
EXPECT_EQ( costmap->get_parameter( "origin_x" ).as_double( ), 6.789 );
EXPECT_EQ( costmap->get_parameter( "origin_y" ).as_double( ), 7.891 );
EXPECT_EQ( costmap->get_parameter( "width" ).as_int( ), 2 );
EXPECT_EQ( costmap->get_parameter( "height" ).as_int( ), 3 );
EXPECT_EQ(
costmap->get_parameter( "footprint" ).as_string( ),
"[[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]]" );
EXPECT_EQ( costmap->get_parameter( "robot_base_frame" ).as_string( ), "test_frame" );
costmap->on_deactivate( rclcpp_lifecycle::State( ) );
costmap->on_cleanup( rclcpp_lifecycle::State( ) );
costmap->on_shutdown( rclcpp_lifecycle::State( ) );
}
1 /*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Dave Hershberger
*********************************************************************/
#include <memory>
#include <string>
#include <vector>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/transform_listener.h"
#include "nav2_costmap_2d/footprint.hpp"
47 class RclCppFixture
{
public:
50 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
51 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
55 class FootprintTestNode
{
public:
58 FootprintTestNode( )
{
// Default footprint padding and footprint radius from Costmap2DROS
testFootprint( 0.01f, 0.1 );
}
64 ~FootprintTestNode( ) {}
66 void testFootprint( double footprint_padding, std::string footprint )
{
footprint_padding_ = footprint_padding;
if ( footprint != "" && footprint != "[]" ) {
std::vector<geometry_msgs::msg::Point> new_footprint;
if ( nav2_costmap_2d::makeFootprintFromString( footprint, new_footprint ) ) {
setRobotFootprint( new_footprint );
} else {
RCLCPP_ERROR( rclcpp::get_logger( "footprint_tester" ), "Invalid footprint string" );
}
}
}
79 void testFootprint( double footprint_padding, double robot_radius )
{
footprint_padding_ = footprint_padding;
setRobotFootprint( nav2_costmap_2d::makeFootprintFromRadius( robot_radius ) );
}
85 std::vector<geometry_msgs::msg::Point> getRobotFootprint( )
{
return footprint_;
}
protected:
91 void setRobotFootprint( const std::vector<geometry_msgs::msg::Point> & points )
{
footprint_ = points;
nav2_costmap_2d::padFootprint( footprint_, footprint_padding_ );
}
double footprint_padding_;
98 std::vector<geometry_msgs::msg::Point> footprint_;
};
101 class TestNode : public ::testing::Test
{
public:
104 TestNode( )
{
footprint_tester_ = std::make_shared<FootprintTestNode>( );
}
109 ~TestNode( ) {}
protected:
112 std::shared_ptr<FootprintTestNode> footprint_tester_;
};
// Start with empty test before updating test footprints
116 TEST_F( TestNode, footprint_empty )
{
// FootprintTestNode cm( "costmap_footprint_empty", *tf_ );
std::vector<geometry_msgs::msg::Point> footprint = footprint_tester_->getRobotFootprint( );
// With no specification of footprint or radius,
// defaults to 0.1 meter radius plus 0.01 meter padding.
EXPECT_EQ( 16u, footprint.size( ) );
EXPECT_NEAR( 0.11f, footprint[0].x, 0.0001 );
EXPECT_NEAR( 0.0f, footprint[0].y, 0.0001 );
EXPECT_EQ( 0.0f, footprint[0].z );
}
129 TEST_F( TestNode, unpadded_footprint_from_string_param )
{
footprint_tester_->testFootprint( 0.0, "[[1, 1], [-1, 1], [-1, -1]]" );
std::vector<geometry_msgs::msg::Point> footprint = footprint_tester_->getRobotFootprint( );
EXPECT_EQ( 3u, footprint.size( ) );
EXPECT_EQ( 1.0f, footprint[0].x );
EXPECT_EQ( 1.0f, footprint[0].y );
EXPECT_EQ( 0.0f, footprint[0].z );
EXPECT_EQ( -1.0f, footprint[1].x );
EXPECT_EQ( 1.0f, footprint[1].y );
EXPECT_EQ( 0.0f, footprint[1].z );
EXPECT_EQ( -1.0f, footprint[2].x );
EXPECT_EQ( -1.0f, footprint[2].y );
EXPECT_EQ( 0.0f, footprint[2].z );
}
149 TEST_F( TestNode, padded_footprint_from_string_param )
{
footprint_tester_->testFootprint( 0.5, "[[1, 1], [-1, 1], [-1, -1]]" );
std::vector<geometry_msgs::msg::Point> footprint = footprint_tester_->getRobotFootprint( );
EXPECT_EQ( 3u, footprint.size( ) );
EXPECT_EQ( 1.5f, footprint[0].x );
EXPECT_EQ( 1.5f, footprint[0].y );
EXPECT_EQ( 0.0f, footprint[0].z );
EXPECT_EQ( -1.5f, footprint[1].x );
EXPECT_EQ( 1.5f, footprint[1].y );
EXPECT_EQ( 0.0f, footprint[1].z );
EXPECT_EQ( -1.5f, footprint[2].x );
EXPECT_EQ( -1.5f, footprint[2].y );
EXPECT_EQ( 0.0f, footprint[2].z );
}
169 TEST_F( TestNode, radius_param )
{
footprint_tester_->testFootprint( 0, 10.0 );
std::vector<geometry_msgs::msg::Point> footprint = footprint_tester_->getRobotFootprint( );
// Circular robot has 16-point footprint auto-generated.
EXPECT_EQ( 16u, footprint.size( ) );
// Check the first point
EXPECT_EQ( 10.0f, footprint[0].x );
EXPECT_EQ( 0.0f, footprint[0].y );
EXPECT_EQ( 0.0f, footprint[0].z );
// Check the 4th point, which should be 90 degrees around the circle from the first.
EXPECT_NEAR( 0.0f, footprint[4].x, 0.0001 );
EXPECT_NEAR( 10.0f, footprint[4].y, 0.0001 );
EXPECT_EQ( 0.0f, footprint[4].z );
}
187 TEST_F( TestNode, footprint_from_same_level_param )
{
footprint_tester_->testFootprint( 0.0, "[[1, 2], [3, 4], [5, 6]]" );
std::vector<geometry_msgs::msg::Point> footprint = footprint_tester_->getRobotFootprint( );
EXPECT_EQ( 3u, footprint.size( ) );
EXPECT_EQ( 1.0f, footprint[0].x );
EXPECT_EQ( 2.0f, footprint[0].y );
EXPECT_EQ( 0.0f, footprint[0].z );
EXPECT_EQ( 3.0f, footprint[1].x );
EXPECT_EQ( 4.0f, footprint[1].y );
EXPECT_EQ( 0.0f, footprint[1].z );
EXPECT_EQ( 5.0f, footprint[2].x );
EXPECT_EQ( 6.0f, footprint[2].y );
EXPECT_EQ( 0.0f, footprint[2].z );
}
1 /*
* Copyright ( c ) 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES ( INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE )
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @author David Lu!!
* Test harness for InflationLayer for Costmap2D
*/
#include <gtest/gtest.h>
#include <cmath>
#include <map>
#include <memory>
#include <string>
#include <vector>
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/layered_costmap.hpp"
#include "nav2_costmap_2d/obstacle_layer.hpp"
#include "nav2_costmap_2d/inflation_layer.hpp"
#include "nav2_costmap_2d/observation_buffer.hpp"
#include "../testing_helper.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
using geometry_msgs::msg::Point;
using nav2_costmap_2d::CellData;
54 class RclCppFixture
{
public:
57 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
58 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
62 class TestNode : public ::testing::Test
{
public:
65 TestNode( ) {}
67 ~TestNode( ) {}
69 std::vector<Point> setRadii(
70 nav2_costmap_2d::LayeredCostmap & layers,
double length, double width );
73 void validatePointInflation(
unsigned int mx, unsigned int my,
75 nav2_costmap_2d::Costmap2D * costmap,
76 std::shared_ptr<nav2_costmap_2d::InflationLayer> & ilayer,
double inflation_radius );
79 void initNode( std::vector<rclcpp::Parameter> parameters );
80 void initNode( double inflation_radius );
82 void waitForMap( std::shared_ptr<nav2_costmap_2d::StaticLayer> & slayer );
protected:
85 nav2_util::LifecycleNode::SharedPtr node_;
};
88 std::vector<Point> TestNode::setRadii(
89 nav2_costmap_2d::LayeredCostmap & layers,
double length, double width )
{
std::vector<Point> polygon;
Point p;
p.x = width;
p.y = length;
polygon.push_back( p );
p.x = width;
p.y = -length;
polygon.push_back( p );
p.x = -width;
p.y = -length;
polygon.push_back( p );
p.x = -width;
p.y = length;
polygon.push_back( p );
layers.setFootprint( polygon );
return polygon;
}
111 void TestNode::waitForMap( std::shared_ptr<nav2_costmap_2d::StaticLayer> & slayer )
{
while ( !slayer->isCurrent( ) ) {
rclcpp::spin_some( node_->get_node_base_interface( ) );
}
}
// Test that a single point gets inflated properly
119 void TestNode::validatePointInflation(
unsigned int mx, unsigned int my,
121 nav2_costmap_2d::Costmap2D * costmap,
122 std::shared_ptr<nav2_costmap_2d::InflationLayer> & ilayer,
double inflation_radius )
{
bool * seen = new bool[costmap->getSizeInCellsX( ) * costmap->getSizeInCellsY( )];
memset( seen, false, costmap->getSizeInCellsX( ) * costmap->getSizeInCellsY( ) * sizeof( bool ) );
std::map<double, std::vector<CellData>> m;
CellData initial( costmap->getIndex( mx, my ), mx, my, mx, my );
m[0].push_back( initial );
for ( std::map<double, std::vector<CellData>>::iterator bin = m.begin( );
bin != m.end( ); ++bin )
{
for ( unsigned int i = 0; i < bin->second.size( ); ++i ) {
const CellData cell = bin->second[i];
if ( !seen[cell.index_] ) {
seen[cell.index_] = true;
unsigned int dx = ( cell.x_ > cell.src_x_ ) ? cell.x_ - cell.src_x_ : cell.src_x_ - cell.x_;
unsigned int dy = ( cell.y_ > cell.src_y_ ) ? cell.y_ - cell.src_y_ : cell.src_y_ - cell.y_;
double dist = std::hypot( dx, dy );
unsigned char expected_cost = ilayer->computeCost( dist );
ASSERT_TRUE( costmap->getCost( cell.x_, cell.y_ ) >= expected_cost );
if ( dist > inflation_radius ) {
continue;
}
if ( dist == bin->first ) {
// Adding to our current bin could cause a reallocation
// Which appears to cause the iterator to get messed up
dist += 0.001;
}
if ( cell.x_ > 0 ) {
CellData data( costmap->getIndex( cell.x_ - 1, cell.y_ ),
cell.x_ - 1, cell.y_, cell.src_x_, cell.src_y_ );
m[dist].push_back( data );
}
if ( cell.y_ > 0 ) {
CellData data( costmap->getIndex( cell.x_, cell.y_ - 1 ),
cell.x_, cell.y_ - 1, cell.src_x_, cell.src_y_ );
m[dist].push_back( data );
}
if ( cell.x_ < costmap->getSizeInCellsX( ) - 1 ) {
CellData data( costmap->getIndex( cell.x_ + 1, cell.y_ ),
cell.x_ + 1, cell.y_, cell.src_x_, cell.src_y_ );
m[dist].push_back( data );
}
if ( cell.y_ < costmap->getSizeInCellsY( ) - 1 ) {
CellData data( costmap->getIndex( cell.x_, cell.y_ + 1 ),
cell.x_, cell.y_ + 1, cell.src_x_, cell.src_y_ );
m[dist].push_back( data );
}
}
}
}
delete[] seen;
}
180 void TestNode::initNode( std::vector<rclcpp::Parameter> parameters )
{
auto options = rclcpp::NodeOptions( );
options.parameter_overrides( parameters );
node_ = std::make_shared<nav2_util::LifecycleNode>(
"inflation_test_node", "", options );
// Declare non-plugin specific costmap parameters
node_->declare_parameter( "map_topic", rclcpp::ParameterValue( std::string( "map" ) ) );
node_->declare_parameter( "track_unknown_space", rclcpp::ParameterValue( false ) );
node_->declare_parameter( "use_maximum", rclcpp::ParameterValue( false ) );
node_->declare_parameter( "lethal_cost_threshold", rclcpp::ParameterValue( 100 ) );
node_->declare_parameter(
"unknown_cost_value",
rclcpp::ParameterValue( static_cast<unsigned char>( 0xff ) ) );
node_->declare_parameter( "trinary_costmap", rclcpp::ParameterValue( true ) );
node_->declare_parameter( "transform_tolerance", rclcpp::ParameterValue( 0.3 ) );
node_->declare_parameter( "observation_sources", rclcpp::ParameterValue( std::string( "" ) ) );
}
201 void TestNode::initNode( double inflation_radius )
{
std::vector<rclcpp::Parameter> parameters;
// Set cost_scaling_factor parameter to 1.0 for inflation layer
parameters.push_back( rclcpp::Parameter( "inflation.cost_scaling_factor", 1.0 ) );
parameters.push_back( rclcpp::Parameter( "inflation.inflation_radius", inflation_radius ) );
initNode( parameters );
}
211 TEST_F( TestNode, testAdjacentToObstacleCanStillMove )
{
initNode( 4.1 );
tf2_ros::Buffer tf( node_->get_clock( ) );
nav2_costmap_2d::LayeredCostmap layers( "frame", false, false );
layers.resizeMap( 10, 10, 1, 0, 0 );
// Footprint with inscribed radius = 2.1
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii( layers, 2.1, 2.3 );
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer( layers, tf, node_, olayer );
std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer( layers, tf, node_, ilayer );
layers.setFootprint( polygon );
addObservation( olayer, 0, 0, MAX_Z );
layers.updateMap( 0, 0, 0 );
nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap( );
// printMap( *costmap );
EXPECT_EQ( nav2_costmap_2d::LETHAL_OBSTACLE, costmap->getCost( 0, 0 ) );
EXPECT_EQ( nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, costmap->getCost( 1, 0 ) );
EXPECT_EQ( nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, costmap->getCost( 2, 0 ) );
EXPECT_TRUE( nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE > costmap->getCost( 3, 0 ) );
EXPECT_TRUE( nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE > costmap->getCost( 2, 1 ) );
EXPECT_EQ( nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, costmap->getCost( 1, 1 ) );
}
243 TEST_F( TestNode, testInflationShouldNotCreateUnknowns )
{
initNode( 4.1 );
tf2_ros::Buffer tf( node_->get_clock( ) );
nav2_costmap_2d::LayeredCostmap layers( "frame", false, false );
layers.resizeMap( 10, 10, 1, 0, 0 );
// Footprint with inscribed radius = 2.1
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii( layers, 2.1, 2.3 );
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer( layers, tf, node_, olayer );
std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer( layers, tf, node_, ilayer );
layers.setFootprint( polygon );
addObservation( olayer, 0, 0, MAX_Z );
layers.updateMap( 0, 0, 0 );
nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap( );
EXPECT_EQ( countValues( *costmap, nav2_costmap_2d::NO_INFORMATION ), 0u );
}
270 TEST_F( TestNode, testInflationInUnkown )
{
std::vector<rclcpp::Parameter> parameters;
// Set cost_scaling_factor parameter to 1.0 for inflation layer
parameters.push_back( rclcpp::Parameter( "inflation.cost_scaling_factor", 1.0 ) );
parameters.push_back( rclcpp::Parameter( "inflation.inflation_radius", 4.1 ) );
parameters.push_back( rclcpp::Parameter( "inflation.inflate_unknown", true ) );
initNode( parameters );
node_->set_parameter( rclcpp::Parameter( "track_unknown_space", true ) );
tf2_ros::Buffer tf( node_->get_clock( ) );
nav2_costmap_2d::LayeredCostmap layers( "frame", false, true );
layers.resizeMap( 9, 9, 1, 0, 0 );
// Footprint with inscribed radius = 2.1
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii( layers, 2.1, 2.3 );
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer( layers, tf, node_, olayer );
std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer( layers, tf, node_, ilayer );
layers.setFootprint( polygon );
addObservation( olayer, 4, 4, MAX_Z, 0.0, 0.0, MAX_Z, true, false );
layers.updateMap( 0, 0, 0 );
nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap( );
// Only the 4 corners of the map should remain unknown
EXPECT_EQ( countValues( *costmap, nav2_costmap_2d::NO_INFORMATION ), 4u );
}
305 TEST_F( TestNode, testInflationAroundUnkown )
{
auto inflation_radius = 4.1;
std::vector<rclcpp::Parameter> parameters;
// Set cost_scaling_factor parameter to 1.0 for inflation layer
parameters.push_back( rclcpp::Parameter( "inflation.cost_scaling_factor", 1.0 ) );
parameters.push_back( rclcpp::Parameter( "inflation.inflation_radius", inflation_radius ) );
parameters.push_back( rclcpp::Parameter( "inflation.inflate_around_unknown", true ) );
initNode( parameters );
node_->set_parameter( rclcpp::Parameter( "track_unknown_space", true ) );
tf2_ros::Buffer tf( node_->get_clock( ) );
nav2_costmap_2d::LayeredCostmap layers( "frame", false, false );
layers.resizeMap( 10, 10, 1, 0, 0 );
// Footprint with inscribed radius = 2.1
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii( layers, 2.1, 2.3 );
std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer( layers, tf, node_, ilayer );
layers.setFootprint( polygon );
layers.updateMap( 0, 0, 0 );
layers.getCostmap( )->setCost( 4, 4, nav2_costmap_2d::NO_INFORMATION );
ilayer->updateCosts( *layers.getCostmap( ), 0, 0, 10, 10 );
validatePointInflation( 4, 4, layers.getCostmap( ), ilayer, inflation_radius );
}
/**
* Test for the cost function correctness with a larger range and different values
*/
340 TEST_F( TestNode, testCostFunctionCorrectness )
{
initNode( 10.5 );
tf2_ros::Buffer tf( node_->get_clock( ) );
nav2_costmap_2d::LayeredCostmap layers( "frame", false, false );
layers.resizeMap( 100, 100, 1, 0, 0 );
// Footprint with inscribed radius = 5.0
// circumscribed radius = 8.0
std::vector<Point> polygon = setRadii( layers, 5.0, 6.25 );
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer( layers, tf, node_, olayer );
std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer( layers, tf, node_, ilayer );
layers.setFootprint( polygon );
addObservation( olayer, 50, 50, MAX_Z );
layers.updateMap( 0, 0, 0 );
nav2_costmap_2d::Costmap2D * map = layers.getCostmap( );
// Verify that the circumscribed cost lower bound is as expected: based on the cost function.
// unsigned char c = ilayer->computeCost( 8.0 );
// ASSERT_EQ( ilayer->getCircumscribedCost( ), c );
for ( unsigned int i = 0; i <= ( unsigned int )ceil( 5.0 ); i++ ) {
// To the right
ASSERT_EQ( map->getCost( 50 + i, 50 ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true );
ASSERT_EQ( map->getCost( 50 + i, 50 ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true );
// To the left
ASSERT_EQ( map->getCost( 50 - i, 50 ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true );
ASSERT_EQ( map->getCost( 50 - i, 50 ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true );
// Down
ASSERT_EQ( map->getCost( 50, 50 + i ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true );
ASSERT_EQ( map->getCost( 50, 50 + i ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true );
// Up
ASSERT_EQ( map->getCost( 50, 50 - i ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true );
ASSERT_EQ( map->getCost( 50, 50 - i ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true );
}
// Verify the normalized cost attenuates as expected
for ( unsigned int i = ( unsigned int )( ceil( 5.0 ) + 1 ); i <= ( unsigned int )ceil( 10.5 ); i++ ) {
unsigned char expectedValue = ilayer->computeCost( i / 1.0 );
ASSERT_EQ( map->getCost( 50 + i, 50 ), expectedValue );
}
// Update with no hits. Should clear ( revert to the static map
/*map->resetMapOutsideWindow( 0, 0, 0.0, 0.0 );
cloud.points.resize( 0 );
p.x = 0.0;
p.y = 0.0;
p.z = MAX_Z;
Observation obs2( p, cloud, 100.0, 100.0 );
std::vector<Observation> obsBuf2;
obsBuf2.push_back( obs2 );
map->updateWorld( 0, 0, obsBuf2, obsBuf2 );
for( unsigned int i = 0; i < 100; i++ )
for( unsigned int j = 0; j < 100; j++ )
ASSERT_EQ( map->getCost( i, j ), nav2_costmap_2d::FREE_SPACE );*/
}
/**
* Test that there is no regression and that costs do not get
* underestimated with the distance-as-key map used to replace
* the previously used priority queue. This is a more thorough
* test of the cost function being correctly applied.
*/
414 TEST_F( TestNode, testInflationOrderCorrectness )
{
const double inflation_radius = 4.1;
initNode( inflation_radius );
tf2_ros::Buffer tf( node_->get_clock( ) );
nav2_costmap_2d::LayeredCostmap layers( "frame", false, false );
layers.resizeMap( 10, 10, 1, 0, 0 );
// Footprint with inscribed radius = 2.1
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii( layers, 2.1, 2.3 );
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer( layers, tf, node_, olayer );
std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer( layers, tf, node_, ilayer );
layers.setFootprint( polygon );
// Add two diagonal cells, they would induce problems under the
// previous implementations
addObservation( olayer, 4, 4, MAX_Z );
addObservation( olayer, 5, 5, MAX_Z );
layers.updateMap( 0, 0, 0 );
validatePointInflation( 4, 4, layers.getCostmap( ), ilayer, inflation_radius );
validatePointInflation( 5, 5, layers.getCostmap( ), ilayer, inflation_radius );
}
/**
* Test inflation for both static and dynamic obstacles
*/
448 TEST_F( TestNode, testInflation )
{
initNode( 1 );
tf2_ros::Buffer tf( node_->get_clock( ) );
nav2_costmap_2d::LayeredCostmap layers( "frame", false, false );
// Footprint with inscribed radius = 2.1
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii( layers, 1, 1 );
std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
addStaticLayer( layers, tf, node_, slayer );
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer( layers, tf, node_, olayer );
std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer( layers, tf, node_, ilayer );
layers.setFootprint( polygon );
nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap( );
waitForMap( slayer );
layers.updateMap( 0, 0, 0 );
// printMap( *costmap );
ASSERT_EQ( countValues( *costmap, nav2_costmap_2d::LETHAL_OBSTACLE ), 20u );
ASSERT_EQ( countValues( *costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE ), 28u );
/*/ Iterate over all id's and verify they are obstacles
for( std::vector<unsigned int>::const_iterator it = occupiedCells.begin( ); it != occupiedCells.end( ); ++it ){
unsigned int ind = *it;
unsigned int x, y;
map.indexToCells( ind, x, y );
ASSERT_EQ( find( occupiedCells, map.getIndex( x, y ) ), true );
ASSERT_EQ( map.getCost( x, y ) == nav2_costmap_2d::LETHAL_OBSTACLE ||
map.getCost( x, y ) == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true );
}*/
addObservation( olayer, 0, 0, 0.4 );
layers.updateMap( 0, 0, 0 );
// It and its 2 neighbors makes 3 obstacles
ASSERT_EQ(
countValues( *costmap, nav2_costmap_2d::LETHAL_OBSTACLE ) +
countValues( *costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE ), 51u );
// @todo Rewrite
// Add an obstacle at <2, 0> which will inflate and refresh to of the other inflated cells
addObservation( olayer, 2, 0 );
layers.updateMap( 0, 0, 0 );
// Now we expect insertions for it, and 2 more neighbors, but not all 5.
// Free space will propagate from
// the origin to the target, clearing the point at <0, 0>,
// but not over-writing the inflation of the obstacle
// at <0, 1>
ASSERT_EQ(
countValues( *costmap, nav2_costmap_2d::LETHAL_OBSTACLE ) +
countValues( *costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE ), 54u );
// Add an obstacle at <1, 9>. This will inflate obstacles around it
addObservation( olayer, 1, 9 );
layers.updateMap( 0, 0, 0 );
ASSERT_EQ( costmap->getCost( 1, 9 ), nav2_costmap_2d::LETHAL_OBSTACLE );
ASSERT_EQ( costmap->getCost( 0, 9 ), nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE );
ASSERT_EQ( costmap->getCost( 2, 9 ), nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE );
// Add an obstacle and verify that it over-writes its inflated status
addObservation( olayer, 0, 9 );
layers.updateMap( 0, 0, 0 );
ASSERT_EQ( costmap->getCost( 0, 9 ), nav2_costmap_2d::LETHAL_OBSTACLE );
}
/**
* Test specific inflation scenario to ensure we do not set inflated obstacles to be raw obstacles.
*/
526 TEST_F( TestNode, testInflation2 )
{
initNode( 1 );
tf2_ros::Buffer tf( node_->get_clock( ) );
nav2_costmap_2d::LayeredCostmap layers( "frame", false, false );
// Footprint with inscribed radius = 2.1
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii( layers, 1, 1 );
std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
addStaticLayer( layers, tf, node_, slayer );
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer( layers, tf, node_, olayer );
std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer( layers, tf, node_, ilayer );
layers.setFootprint( polygon );
waitForMap( slayer );
// Creat a small L-Shape all at once
addObservation( olayer, 1, 1, MAX_Z );
addObservation( olayer, 2, 1, MAX_Z );
addObservation( olayer, 2, 2, MAX_Z );
layers.updateMap( 0, 0, 0 );
nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap( );
// printMap( *costmap );
ASSERT_EQ( costmap->getCost( 2, 3 ), nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE );
ASSERT_EQ( costmap->getCost( 3, 3 ), nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE );
}
/**
* Test inflation behavior, starting with an empty map
*/
564 TEST_F( TestNode, testInflation3 )
{
initNode( 3 );
tf2_ros::Buffer tf( node_->get_clock( ) );
nav2_costmap_2d::LayeredCostmap layers( "frame", false, false );
layers.resizeMap( 10, 10, 1, 0, 0 );
// 1 2 3
std::vector<Point> polygon = setRadii( layers, 1, 1.75 );
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer( layers, tf, node_, olayer );
std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer( layers, tf, node_, ilayer );
layers.setFootprint( polygon );
// There should be no occupied cells
nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap( );
ASSERT_EQ( countValues( *costmap, nav2_costmap_2d::LETHAL_OBSTACLE ), 0u );
ASSERT_EQ( countValues( *costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE ), 0u );
printMap( *costmap );
// Add an obstacle at 5, 5
addObservation( olayer, 5, 5, MAX_Z );
layers.updateMap( 0, 0, 0 );
printMap( *costmap );
// Test fails because updated cell value is 0
ASSERT_EQ( countValues( *costmap, nav2_costmap_2d::FREE_SPACE, false ), 29u );
ASSERT_EQ( countValues( *costmap, nav2_costmap_2d::LETHAL_OBSTACLE ), 1u );
ASSERT_EQ( countValues( *costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE ), 4u );
// Update again - should see no change
layers.updateMap( 0, 0, 0 );
ASSERT_EQ( countValues( *costmap, nav2_costmap_2d::FREE_SPACE, false ), 29u );
ASSERT_EQ( countValues( *costmap, nav2_costmap_2d::LETHAL_OBSTACLE ), 1u );
ASSERT_EQ( countValues( *costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE ), 4u );
}
/**
* Test dynamic parameter setting of inflation layer
*/
608 TEST_F( TestNode, testDynParamsSet )
{
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "test_costmap" );
costmap->set_parameter( rclcpp::Parameter( "global_frame", std::string( "base_link" ) ) );
costmap->on_configure( rclcpp_lifecycle::State( ) );
costmap->on_activate( rclcpp_lifecycle::State( ) );
auto parameter_client = std::make_shared<rclcpp::AsyncParametersClient>(
costmap->get_node_base_interface( ), costmap->get_node_topics_interface( ),
costmap->get_node_graph_interface( ),
costmap->get_node_services_interface( ) );
auto results = parameter_client->set_parameters_atomically(
{
rclcpp::Parameter( "inflation_layer.inflation_radius", 0.0 ),
rclcpp::Parameter( "inflation_layer.cost_scaling_factor", 0.0 ),
rclcpp::Parameter( "inflation_layer.inflate_unknown", true ),
rclcpp::Parameter( "inflation_layer.inflate_around_unknown", true ),
rclcpp::Parameter( "inflation_layer.enabled", false )
} );
rclcpp::spin_until_future_complete(
costmap->get_node_base_interface( ),
results );
EXPECT_EQ( costmap->get_parameter( "inflation_layer.inflation_radius" ).as_double( ), 0.0 );
EXPECT_EQ( costmap->get_parameter( "inflation_layer.cost_scaling_factor" ).as_double( ), 0.0 );
EXPECT_EQ( costmap->get_parameter( "inflation_layer.inflate_unknown" ).as_bool( ), true );
EXPECT_EQ( costmap->get_parameter( "inflation_layer.inflate_around_unknown" ).as_bool( ), true );
EXPECT_EQ( costmap->get_parameter( "inflation_layer.enabled" ).as_bool( ), false );
costmap->on_deactivate( rclcpp_lifecycle::State( ) );
costmap->on_cleanup( rclcpp_lifecycle::State( ) );
costmap->on_shutdown( rclcpp_lifecycle::State( ) );
}
1 /*
* Copyright ( c ) 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES ( INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE )
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @author David Lu!!
* Test harness for ObstacleLayer for Costmap2D
*/
#include <memory>
#include <string>
#include <algorithm>
#include <utility>
#include "gtest/gtest.h"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/layered_costmap.hpp"
#include "nav2_costmap_2d/observation_buffer.hpp"
#include "../testing_helper.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
using std::begin;
using std::end;
using std::for_each;
using std::all_of;
using std::none_of;
using std::pair;
using std::string;
55 class RclCppFixture
{
public:
58 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
59 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
63 class TestLifecycleNode : public nav2_util::LifecycleNode
{
public:
66 explicit TestLifecycleNode( const string & name )
: nav2_util::LifecycleNode( name )
{
}
71 nav2_util::CallbackReturn on_configure( const rclcpp_lifecycle::State & )
{
return nav2_util::CallbackReturn::SUCCESS;
}
76 nav2_util::CallbackReturn on_activate( const rclcpp_lifecycle::State & )
{
return nav2_util::CallbackReturn::SUCCESS;
}
81 nav2_util::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & )
{
return nav2_util::CallbackReturn::SUCCESS;
}
86 nav2_util::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & )
{
return nav2_util::CallbackReturn::SUCCESS;
}
91 nav2_util::CallbackReturn onShutdown( const rclcpp_lifecycle::State & )
{
return nav2_util::CallbackReturn::SUCCESS;
}
96 nav2_util::CallbackReturn onError( const rclcpp_lifecycle::State & )
{
return nav2_util::CallbackReturn::SUCCESS;
}
};
102 class TestNode : public ::testing::Test
{
public:
105 TestNode( )
{
node_ = std::make_shared<TestLifecycleNode>( "obstacle_test_node" );
node_->declare_parameter( "map_topic", rclcpp::ParameterValue( std::string( "map" ) ) );
node_->declare_parameter( "track_unknown_space", rclcpp::ParameterValue( false ) );
node_->declare_parameter( "use_maximum", rclcpp::ParameterValue( false ) );
node_->declare_parameter( "lethal_cost_threshold", rclcpp::ParameterValue( 100 ) );
node_->declare_parameter(
"unknown_cost_value",
rclcpp::ParameterValue( static_cast<unsigned char>( 0xff ) ) );
node_->declare_parameter( "trinary_costmap", rclcpp::ParameterValue( true ) );
node_->declare_parameter( "transform_tolerance", rclcpp::ParameterValue( 0.3 ) );
node_->declare_parameter( "observation_sources", rclcpp::ParameterValue( std::string( "" ) ) );
}
120 ~TestNode( ) {}
protected:
123 std::shared_ptr<TestLifecycleNode> node_;
};
/*
* For reference, the static map looks like this:
*
* 0 0 0 0 0 0 0 254 254 254
*
* 0 0 0 0 0 0 0 254 254 254
*
* 0 0 0 254 254 254 0 0 0 0
*
* 0 0 0 0 0 0 0 0 0 0
*
* 0 0 0 0 0 0 0 0 0 0
*
* 0 0 0 0 254 0 0 254 254 254
*
* 0 0 0 0 254 0 0 254 254 254
*
* 0 0 0 0 0 0 0 254 254 254
*
* 0 0 0 0 0 0 0 0 0 0
*
* 0 0 0 0 0 0 0 0 0 0
*
* upper left is 0, 0, lower right is 9, 9
*/
#if ( 0 )
/**
* Test for ray tracing free space
*/
156 TEST_F( TestNode, testRaytracing ) {
tf2_ros::Buffer tf( node_->get_clock( ) );
nav2_costmap_2d::LayeredCostmap layers( "frame", false, false );
addStaticLayer( layers, tf, node_ );
auto olayer = addObstacleLayer( layers, tf, node_ );
// Add a point at 0, 0, 0
addObservation( olayer, 0.0, 0.0, MAX_Z / 2, 0, 0, MAX_Z / 2 );
// This actually puts the LETHAL ( 254 ) point in the costmap at ( 0, 0 )
layers.updateMap( 0, 0, 0 ); // 0, 0, 0 is robot pose
// printMap( *( layers.getCostmap( ) ) );
int lethal_count = countValues( *( layers.getCostmap( ) ), nav2_costmap_2d::LETHAL_OBSTACLE );
// We expect just one obstacle to be added ( 20 in static map )
ASSERT_EQ( lethal_count, 21 );
}
/**
* Test for ray tracing free space
*/
179 TEST_F( TestNode, testRaytracing2 ) {
tf2_ros::Buffer tf( node_->get_clock( ) );
nav2_costmap_2d::LayeredCostmap layers( "frame", false, false );
addStaticLayer( layers, tf, node_ );
auto olayer = addObstacleLayer( layers, tf, node_ );
// If we print map now, it is 10x10 all value 0
// printMap( *( layers.getCostmap( ) ) );
// Update will fill in the costmap with the static map
layers.updateMap( 0, 0, 0 );
// If we print the map now, we get the static map
// printMap( *( layers.getCostmap( ) ) );
// Static map has 20 LETHAL cells ( see diagram above )
int obs_before = countValues( *( layers.getCostmap( ) ), nav2_costmap_2d::LETHAL_OBSTACLE );
ASSERT_EQ( obs_before, 20 );
// The sensor origin will be <0, 0>. So if we add an obstacle at 9, 9,
// we would expect cells <0, 0> thru <8, 8> to be traced through
// however the static map is not cleared by obstacle layer
addObservation( olayer, 9.5, 9.5, MAX_Z / 2, 0.5, 0.5, MAX_Z / 2 );
layers.updateMap( 0, 0, 0 );
// If we print map now, we have static map + <9, 9> is LETHAL
// printMap( *( layers.getCostmap( ) ) );
int obs_after = countValues( *( layers.getCostmap( ) ), nav2_costmap_2d::LETHAL_OBSTACLE );
// Change from previous test:
// No obstacles from the static map will be cleared, so the
// net change is +1.
ASSERT_EQ( obs_after, obs_before + 1 );
// Fill in the diagonal, <7, 7> and <9, 9> already filled in, <0, 0> is robot
for ( int i = 0; i < olayer->getSizeInCellsY( ); ++i ) {
olayer->setCost( i, i, nav2_costmap_2d::LETHAL_OBSTACLE );
}
// This will updateBounds, which will raytrace the static observation added
// above, thus clearing out the diagonal again!
layers.updateMap( 0, 0, 0 );
// Map now has diagonal except <0, 0> filled with LETHAL ( 254 )
// printMap( *( layers.getCostmap( ) ) );
int with_static = countValues( *( layers.getCostmap( ) ), nav2_costmap_2d::LETHAL_OBSTACLE );
// Should thus be the same
ASSERT_EQ( with_static, obs_after );
// If 21 are filled, 79 should be free
ASSERT_EQ( 79, countValues( *( layers.getCostmap( ) ), nav2_costmap_2d::FREE_SPACE ) );
}
/**
* Test for wave interference
*/
234 TEST_F( TestNode, testWaveInterference ) {
tf2_ros::Buffer tf( node_->get_clock( ) );
node_->set_parameter( rclcpp::Parameter( "track_unknown_space", true ) );
// Start with an empty map, no rolling window, tracking unknown
nav2_costmap_2d::LayeredCostmap layers( "frame", false, true );
layers.resizeMap( 10, 10, 1, 0, 0 );
auto olayer = addObstacleLayer( layers, tf, node_ );
// If we print map now, it is 10x10, all cells are 255 ( NO_INFORMATION )
// printMap( *( layers.getCostmap( ) ) );
// Lay out 3 obstacles in a line - along the diagonal, separated by a cell.
addObservation( olayer, 3.0, 3.0, MAX_Z );
addObservation( olayer, 5.0, 5.0, MAX_Z );
addObservation( olayer, 7.0, 7.0, MAX_Z );
layers.updateMap( 0, 0, 0 );
nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap( );
// 3 obstacle cells are filled, <1, 1>, <2, 2>, <4, 4> and <6, 6> are now free
// <0, 0> is footprint and is free
// printMap( *costmap );
ASSERT_EQ( 3, countValues( *costmap, nav2_costmap_2d::LETHAL_OBSTACLE ) );
ASSERT_EQ( 92, countValues( *costmap, nav2_costmap_2d::NO_INFORMATION ) );
ASSERT_EQ( 5, countValues( *costmap, nav2_costmap_2d::FREE_SPACE ) );
}
/**
* Make sure we ignore points outside of our z threshold
*/
263 TEST_F( TestNode, testZThreshold ) {
tf2_ros::Buffer tf( node_->get_clock( ) );
// Start with an empty map
nav2_costmap_2d::LayeredCostmap layers( "frame", false, true );
layers.resizeMap( 10, 10, 1, 0, 0 );
auto olayer = addObstacleLayer( layers, tf, node_ );
// A point cloud with 2 points falling in a cell with a non-lethal cost
addObservation( olayer, 0.0, 5.0, 0.4 );
addObservation( olayer, 1.0, 5.0, 2.2 );
layers.updateMap( 0, 0, 0 );
nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap( );
ASSERT_EQ( countValues( *costmap, nav2_costmap_2d::LETHAL_OBSTACLE ), 1 );
}
/**
* Verify that dynamic obstacles are added
*/
284 TEST_F( TestNode, testDynamicObstacles ) {
tf2_ros::Buffer tf( node_->get_clock( ) );
nav2_costmap_2d::LayeredCostmap layers( "frame", false, false );
addStaticLayer( layers, tf, node_ );
auto olayer = addObstacleLayer( layers, tf, node_ );
// Add a point cloud and verify its insertion. There should be only one new one
addObservation( olayer, 0.0, 0.0 );
addObservation( olayer, 0.0, 0.0 );
addObservation( olayer, 0.0, 0.0 );
layers.updateMap( 0, 0, 0 );
nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap( );
// Should now have 1 insertion and no deletions
ASSERT_EQ( countValues( *costmap, nav2_costmap_2d::LETHAL_OBSTACLE ), 21 );
// Repeating the call - we should see no insertions or deletions
ASSERT_EQ( countValues( *costmap, nav2_costmap_2d::LETHAL_OBSTACLE ), 21 );
}
/**
* Verify that if we add a point that is already a static obstacle we do not end up with a new ostacle
*/
309 TEST_F( TestNode, testMultipleAdditions ) {
tf2_ros::Buffer tf( node_->get_clock( ) );
nav2_costmap_2d::LayeredCostmap layers( "frame", false, false );
addStaticLayer( layers, tf, node_ );
auto olayer = addObstacleLayer( layers, tf, node_ );
// A point cloud with one point that falls within an existing obstacle
addObservation( olayer, 9.5, 0.0 );
layers.updateMap( 0, 0, 0 );
nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap( );
// printMap( *costmap );
ASSERT_EQ( countValues( *costmap, nav2_costmap_2d::LETHAL_OBSTACLE ), 20 );
}
#endif
/**
* Verify correct init/reset cycling of layer
*/
328 TEST_F( TestNode, testRepeatedResets ) {
tf2_ros::Buffer tf( node_->get_clock( ) );
nav2_costmap_2d::LayeredCostmap layers( "frame", false, false );
std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
addStaticLayer( layers, tf, node_, slayer );
// TODO( orduno ) Add obstacle layer
// Define a node-level parameter
pair<string, string> node_dummy = {"node_dummy_param", "node_dummy_val"};
node_->declare_parameter( node_dummy.first, rclcpp::ParameterValue( node_dummy.second ) );
// Define a layer-level parameter
pair<string, string> layer_dummy = {"dummy_param", "dummy_val"};
// Set parameters
auto plugins = layers.getPlugins( );
for_each(
begin( *plugins ), end( *plugins ), [&layer_dummy]( const auto & plugin ) {
string layer_param = layer_dummy.first + "_" + plugin->getName( );
// Notice we are using Layer::declareParameter
plugin->declareParameter( layer_param, rclcpp::ParameterValue( layer_dummy.second ) );
} );
// Check that all parameters have been set
// node-level param
ASSERT_TRUE( node_->has_parameter( node_dummy.first ) );
// layer-level param
ASSERT_TRUE(
all_of(
begin( *plugins ), end( *plugins ), [&layer_dummy]( const auto & plugin ) {
string layer_param = layer_dummy.first + "_" + plugin->getName( );
return plugin->hasParameter( layer_param );
} ) );
// Reset all layers. Parameters should be declared if not declared, otherwise skipped.
ASSERT_NO_THROW(
for_each(
begin( *plugins ), end( *plugins ), []( const auto & plugin ) {
plugin->reset( );
} ) );
}
/**
* Test for ray tracing free space
*/
378 TEST_F( TestNode, testRaytracing ) {
tf2_ros::Buffer tf( node_->get_clock( ) );
nav2_costmap_2d::LayeredCostmap layers( "frame", false, false );
layers.resizeMap( 10, 10, 1, 0, 0 );
std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
addStaticLayer( layers, tf, node_, slayer );
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer = nullptr;
addObstacleLayer( layers, tf, node_, olayer );
addObservation( olayer, 0.0, 0.0, MAX_Z / 2, 0, 0, MAX_Z / 2 );
// This actually puts the LETHAL ( 254 ) point in the costmap at ( 0, 0 )
layers.updateMap( 0, 0, 0 ); // 0, 0, 0 is robot pose
// printMap( *( layers.getCostmap( ) ) );
int lethal_count = countValues( *( layers.getCostmap( ) ), nav2_costmap_2d::LETHAL_OBSTACLE );
ASSERT_EQ( lethal_count, 1 );
addObservation( olayer, 1.0, 1.0, MAX_Z / 2, 0, 0, MAX_Z / 2, true, true, 100.0, 5.0, 100.0, 5.0 );
// This actually puts the LETHAL ( 254 ) point in the costmap at ( 0, 0 )
layers.updateMap( 0, 0, 0 ); // 0, 0, 0 is robot pose
// printMap( *( layers.getCostmap( ) ) );
// New observation should not be recorded as min_range is higher than obstacle range
lethal_count = countValues( *( layers.getCostmap( ) ), nav2_costmap_2d::LETHAL_OBSTACLE );
ASSERT_EQ( lethal_count, 1 );
}
/**
* Test dynamic parameter setting of obstacle layer
*/
414 TEST_F( TestNode, testDynParamsSetObstacle )
{
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "test_costmap" );
// Add obstacle layer
std::vector<std::string> plugins_str;
plugins_str.push_back( "obstacle_layer" );
costmap->set_parameter( rclcpp::Parameter( "plugins", plugins_str ) );
costmap->declare_parameter(
"obstacle_layer.plugin",
rclcpp::ParameterValue( std::string( "nav2_costmap_2d::ObstacleLayer" ) ) );
costmap->set_parameter( rclcpp::Parameter( "global_frame", std::string( "base_link" ) ) );
costmap->on_configure( rclcpp_lifecycle::State( ) );
costmap->on_activate( rclcpp_lifecycle::State( ) );
auto parameter_client = std::make_shared<rclcpp::AsyncParametersClient>(
costmap->get_node_base_interface( ), costmap->get_node_topics_interface( ),
costmap->get_node_graph_interface( ),
costmap->get_node_services_interface( ) );
auto results = parameter_client->set_parameters_atomically(
{
rclcpp::Parameter( "obstacle_layer.combination_method", 5 ),
rclcpp::Parameter( "obstacle_layer.max_obstacle_height", 4.0 ),
rclcpp::Parameter( "obstacle_layer.enabled", false ),
rclcpp::Parameter( "obstacle_layer.footprint_clearing_enabled", false )
} );
rclcpp::spin_until_future_complete(
costmap->get_node_base_interface( ),
results );
EXPECT_EQ( costmap->get_parameter( "obstacle_layer.combination_method" ).as_int( ), 5 );
EXPECT_EQ( costmap->get_parameter( "obstacle_layer.max_obstacle_height" ).as_double( ), 4.0 );
EXPECT_EQ( costmap->get_parameter( "obstacle_layer.enabled" ).as_bool( ), false );
EXPECT_EQ( costmap->get_parameter( "obstacle_layer.footprint_clearing_enabled" ).as_bool( ), false );
costmap->on_deactivate( rclcpp_lifecycle::State( ) );
costmap->on_cleanup( rclcpp_lifecycle::State( ) );
costmap->on_shutdown( rclcpp_lifecycle::State( ) );
}
/**
* Test dynamic parameter setting of voxel layer
*/
461 TEST_F( TestNode, testDynParamsSetVoxel )
{
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "test_costmap" );
// Add voxel layer
std::vector<std::string> plugins_str;
plugins_str.push_back( "voxel_layer" );
costmap->set_parameter( rclcpp::Parameter( "plugins", plugins_str ) );
costmap->declare_parameter(
"voxel_layer.plugin",
rclcpp::ParameterValue( std::string( "nav2_costmap_2d::VoxelLayer" ) ) );
costmap->set_parameter( rclcpp::Parameter( "global_frame", std::string( "base_link" ) ) );
costmap->on_configure( rclcpp_lifecycle::State( ) );
costmap->on_activate( rclcpp_lifecycle::State( ) );
auto parameter_client = std::make_shared<rclcpp::AsyncParametersClient>(
costmap->get_node_base_interface( ), costmap->get_node_topics_interface( ),
costmap->get_node_graph_interface( ),
costmap->get_node_services_interface( ) );
auto results = parameter_client->set_parameters_atomically(
{
rclcpp::Parameter( "voxel_layer.combination_method", 0 ),
rclcpp::Parameter( "voxel_layer.mark_threshold", 1 ),
rclcpp::Parameter( "voxel_layer.unknown_threshold", 10 ),
rclcpp::Parameter( "voxel_layer.z_resolution", 0.4 ),
rclcpp::Parameter( "voxel_layer.origin_z", 1.0 ),
rclcpp::Parameter( "voxel_layer.z_voxels", 14 ),
rclcpp::Parameter( "voxel_layer.max_obstacle_height", 4.0 ),
rclcpp::Parameter( "voxel_layer.footprint_clearing_enabled", false ),
rclcpp::Parameter( "voxel_layer.enabled", false ),
rclcpp::Parameter( "voxel_layer.publish_voxel_map", true )
} );
rclcpp::spin_until_future_complete(
costmap->get_node_base_interface( ),
results );
EXPECT_EQ( costmap->get_parameter( "voxel_layer.combination_method" ).as_int( ), 0 );
EXPECT_EQ( costmap->get_parameter( "voxel_layer.mark_threshold" ).as_int( ), 1 );
EXPECT_EQ( costmap->get_parameter( "voxel_layer.unknown_threshold" ).as_int( ), 10 );
EXPECT_EQ( costmap->get_parameter( "voxel_layer.z_resolution" ).as_double( ), 0.4 );
EXPECT_EQ( costmap->get_parameter( "voxel_layer.origin_z" ).as_double( ), 1.0 );
EXPECT_EQ( costmap->get_parameter( "voxel_layer.z_voxels" ).as_int( ), 14 );
EXPECT_EQ( costmap->get_parameter( "voxel_layer.max_obstacle_height" ).as_double( ), 4.0 );
EXPECT_EQ( costmap->get_parameter( "voxel_layer.footprint_clearing_enabled" ).as_bool( ), false );
EXPECT_EQ( costmap->get_parameter( "voxel_layer.enabled" ).as_bool( ), false );
EXPECT_EQ( costmap->get_parameter( "voxel_layer.publish_voxel_map" ).as_bool( ), true );
costmap->on_deactivate( rclcpp_lifecycle::State( ) );
costmap->on_cleanup( rclcpp_lifecycle::State( ) );
costmap->on_shutdown( rclcpp_lifecycle::State( ) );
}
/**
* Test dynamic parameter setting of static layer
*/
520 TEST_F( TestNode, testDynParamsSetStatic )
{
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "test_costmap" );
costmap->set_parameter( rclcpp::Parameter( "global_frame", std::string( "base_link" ) ) );
costmap->on_configure( rclcpp_lifecycle::State( ) );
costmap->on_activate( rclcpp_lifecycle::State( ) );
auto parameter_client = std::make_shared<rclcpp::AsyncParametersClient>(
costmap->get_node_base_interface( ), costmap->get_node_topics_interface( ),
costmap->get_node_graph_interface( ),
costmap->get_node_services_interface( ) );
auto results = parameter_client->set_parameters_atomically(
{
rclcpp::Parameter( "static_layer.transform_tolerance", 1.0 ),
rclcpp::Parameter( "static_layer.enabled", false ),
rclcpp::Parameter( "static_layer.map_subscribe_transient_local", false ),
rclcpp::Parameter( "static_layer.map_topic", "dynamic_topic" ),
rclcpp::Parameter( "static_layer.subscribe_to_updates", true )
} );
rclcpp::spin_until_future_complete(
costmap->get_node_base_interface( ),
results );
EXPECT_EQ( costmap->get_parameter( "static_layer.transform_tolerance" ).as_double( ), 1.0 );
EXPECT_EQ( costmap->get_parameter( "static_layer.enabled" ).as_bool( ), false );
EXPECT_EQ( costmap->get_parameter( "static_layer.map_subscribe_transient_local" ).as_bool( ), false );
EXPECT_EQ( costmap->get_parameter( "static_layer.map_topic" ).as_string( ), "dynamic_topic" );
EXPECT_EQ( costmap->get_parameter( "static_layer.subscribe_to_updates" ).as_bool( ), true );
costmap->on_deactivate( rclcpp_lifecycle::State( ) );
costmap->on_cleanup( rclcpp_lifecycle::State( ) );
costmap->on_shutdown( rclcpp_lifecycle::State( ) );
}
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2020, Bytes Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <memory>
#include <string>
#include <algorithm>
#include <utility>
#include <vector>
#include "gtest/gtest.h"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/layered_costmap.hpp"
#include "nav2_costmap_2d/observation_buffer.hpp"
#include "../testing_helper.hpp"
#include "sensor_msgs/msg/range.hpp"
using std::begin;
using std::end;
using std::for_each;
using std::all_of;
using std::none_of;
using std::pair;
using std::string;
56 class RclCppFixture
{
public:
59 RclCppFixture( )
{
rclcpp::init( 0, nullptr );
}
64 ~RclCppFixture( )
{
rclcpp::shutdown( );
}
};
RclCppFixture g_rclcppfixture;
72 class TestLifecycleNode : public nav2_util::LifecycleNode
{
public:
75 explicit TestLifecycleNode( const string & name )
: nav2_util::LifecycleNode( name )
{
}
80 nav2_util::CallbackReturn on_configure( const rclcpp_lifecycle::State & )
{
return nav2_util::CallbackReturn::SUCCESS;
}
85 nav2_util::CallbackReturn on_activate( const rclcpp_lifecycle::State & )
{
return nav2_util::CallbackReturn::SUCCESS;
}
90 nav2_util::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & )
{
return nav2_util::CallbackReturn::SUCCESS;
}
95 nav2_util::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & )
{
return nav2_util::CallbackReturn::SUCCESS;
}
100 nav2_util::CallbackReturn onShutdown( const rclcpp_lifecycle::State & )
{
return nav2_util::CallbackReturn::SUCCESS;
}
105 nav2_util::CallbackReturn onError( const rclcpp_lifecycle::State & )
{
return nav2_util::CallbackReturn::SUCCESS;
}
};
111 class TestNode : public ::testing::Test
{
public:
114 TestNode( )
: node_( std::make_shared<TestLifecycleNode>( "range_test_node" ) ),
tf_( node_->get_clock( ) )
{
tf_.setUsingDedicatedThread( true );
// Standard non-plugin specific parameters
node_->declare_parameter( "map_topic", rclcpp::ParameterValue( std::string( "map" ) ) );
node_->declare_parameter( "track_unknown_space", rclcpp::ParameterValue( false ) );
node_->declare_parameter( "use_maximum", rclcpp::ParameterValue( false ) );
node_->declare_parameter( "lethal_cost_threshold", rclcpp::ParameterValue( 100 ) );
node_->declare_parameter(
"unknown_cost_value",
rclcpp::ParameterValue( static_cast<unsigned char>( 0xff ) ) );
node_->declare_parameter( "trinary_costmap", rclcpp::ParameterValue( true ) );
node_->declare_parameter( "transform_tolerance", rclcpp::ParameterValue( 0.3 ) );
node_->declare_parameter( "observation_sources", rclcpp::ParameterValue( std::string( "range" ) ) );
node_->declare_parameter( "global_frame", rclcpp::ParameterValue( std::string( "map" ) ) );
// Range sensor specific parameters
node_->declare_parameter(
"range.topics",
rclcpp::ParameterValue(
std::vector<std::string>{"/range/topic"} ) );
node_->declare_parameter( "range.phi", rclcpp::ParameterValue( 1.2 ) );
node_->declare_parameter( "range.clear_on_max_reading", rclcpp::ParameterValue( true ) );
}
142 ~TestNode( ) {}
protected:
145 std::shared_ptr<TestLifecycleNode> node_;
146 tf2_ros::Buffer tf_;
};
// Test clearing at max range
150 TEST_F( TestNode, testClearingAtMaxRange ) {
geometry_msgs::msg::TransformStamped transform;
transform.header.stamp = node_->now( );
transform.header.frame_id = "frame";
transform.child_frame_id = "base_link";
transform.transform.translation.y = 5;
transform.transform.translation.x = 2;
tf_.setTransform( transform, "default_authority", true );
nav2_costmap_2d::LayeredCostmap layers( "frame", false, false );
layers.resizeMap( 10, 10, 1, 0, 0 );
std::shared_ptr<nav2_costmap_2d::RangeSensorLayer> rlayer{nullptr};
addRangeLayer( layers, tf_, node_, rlayer );
sensor_msgs::msg::Range msg;
msg.min_range = 1.0;
msg.max_range = 7.0;
msg.range = 2.0;
msg.header.stamp = node_->now( );
msg.header.frame_id = "base_link";
msg.radiation_type = msg.ULTRASOUND;
msg.field_of_view = 0.174533; // 10 deg
rlayer->bufferIncomingRangeMsg( std::make_shared<sensor_msgs::msg::Range>( msg ) );
layers.updateMap( 0, 0, 0 ); // 0, 0, 0 is robot pose
// printMap( *( layers.getCostmap( ) ) );
ASSERT_EQ( layers.getCostmap( )->getCost( 4, 5 ), 254 );
msg.range = 7.0;
msg.header.stamp = node_->now( );
rlayer->bufferIncomingRangeMsg( std::make_shared<sensor_msgs::msg::Range>( msg ) );
layers.updateMap( 0, 0, 0 ); // 0, 0, 0 is robot pose
// printMap( *( layers.getCostmap( ) ) );
ASSERT_EQ( layers.getCostmap( )->getCost( 4, 5 ), 0 );
}
// Testing fixed scan with robot forward motion
190 TEST_F( TestNode, testProbabalisticModelForward ) {
geometry_msgs::msg::TransformStamped transform;
transform.header.stamp = node_->now( );
transform.header.frame_id = "frame";
transform.child_frame_id = "base_link";
transform.transform.translation.y = 5;
transform.transform.translation.x = 2;
tf_.setTransform( transform, "default_authority", true );
nav2_costmap_2d::LayeredCostmap layers( "frame", false, false );
layers.resizeMap( 10, 10, 1, 0, 0 );
std::shared_ptr<nav2_costmap_2d::RangeSensorLayer> rlayer{nullptr};
addRangeLayer( layers, tf_, node_, rlayer );
sensor_msgs::msg::Range msg;
msg.min_range = 1.0;
msg.max_range = 10.0;
msg.range = 3.0;
msg.header.stamp = node_->now( );
msg.header.frame_id = "base_link";
msg.radiation_type = msg.ULTRASOUND;
msg.field_of_view = 0.174533; // 10 deg
rlayer->bufferIncomingRangeMsg( std::make_shared<sensor_msgs::msg::Range>( msg ) );
layers.updateMap( 0, 0, 0 ); // 0, 0, 0 is robot pose
// printMap( *( layers.getCostmap( ) ) );
rlayer->bufferIncomingRangeMsg( std::make_shared<sensor_msgs::msg::Range>( msg ) );
transform.transform.translation.y = 5;
transform.transform.translation.x = 4;
tf_.setTransform( transform, "default_authority", true );
layers.updateMap( 0, 0, 0 ); // 0, 0, 0 is robot pose
// printMap( *( layers.getCostmap( ) ) );
rlayer->bufferIncomingRangeMsg( std::make_shared<sensor_msgs::msg::Range>( msg ) );
transform.transform.translation.y = 5;
transform.transform.translation.x = 6;
tf_.setTransform( transform, "default_authority", true );
layers.updateMap( 0, 0, 0 ); // 0, 0, 0 is robot pose
// printMap( *( layers.getCostmap( ) ) );
ASSERT_EQ( layers.getCostmap( )->getCost( 5, 5 ), 254 );
ASSERT_EQ( layers.getCostmap( )->getCost( 6, 5 ), 0 );
ASSERT_EQ( layers.getCostmap( )->getCost( 7, 5 ), 254 );
ASSERT_EQ( layers.getCostmap( )->getCost( 8, 5 ), 0 );
ASSERT_EQ( layers.getCostmap( )->getCost( 9, 5 ), 254 );
}
// Testing fixed motion with downward movement
243 TEST_F( TestNode, testProbabalisticModelDownward ) {
geometry_msgs::msg::TransformStamped transform;
transform.header.stamp = node_->now( );
transform.header.frame_id = "frame";
transform.child_frame_id = "base_link";
transform.transform.translation.y = 3;
transform.transform.translation.x = 2;
tf_.setTransform( transform, "default_authority", true );
nav2_costmap_2d::LayeredCostmap layers( "frame", false, false );
layers.resizeMap( 10, 10, 1, 0, 0 );
std::shared_ptr<nav2_costmap_2d::RangeSensorLayer> rlayer{nullptr};
addRangeLayer( layers, tf_, node_, rlayer );
sensor_msgs::msg::Range msg;
msg.min_range = 1.0;
msg.max_range = 10.0;
msg.range = 1.0;
msg.header.stamp = node_->now( );
msg.header.frame_id = "base_link";
msg.radiation_type = msg.ULTRASOUND;
msg.field_of_view = 0.174533; // 10 deg
rlayer->bufferIncomingRangeMsg( std::make_shared<sensor_msgs::msg::Range>( msg ) );
layers.updateMap( 0, 0, 0 ); // 0, 0, 0 is robot pose
// printMap( *( layers.getCostmap( ) ) );
rlayer->bufferIncomingRangeMsg( std::make_shared<sensor_msgs::msg::Range>( msg ) );
transform.transform.translation.y = 5;
transform.transform.translation.x = 2;
tf_.setTransform( transform, "default_authority", true );
layers.updateMap( 0, 0, 0 ); // 0, 0, 0 is robot pose
// printMap( *( layers.getCostmap( ) ) );
rlayer->bufferIncomingRangeMsg( std::make_shared<sensor_msgs::msg::Range>( msg ) );
transform.transform.translation.y = 7;
transform.transform.translation.x = 2;
tf_.setTransform( transform, "default_authority", true );
layers.updateMap( 0, 0, 0 ); // 0, 0, 0 is robot pose
// printMap( *( layers.getCostmap( ) ) );
ASSERT_EQ( layers.getCostmap( )->getCost( 3, 3 ), 254 );
ASSERT_EQ( layers.getCostmap( )->getCost( 3, 4 ), 0 );
ASSERT_EQ( layers.getCostmap( )->getCost( 3, 5 ), 254 );
ASSERT_EQ( layers.getCostmap( )->getCost( 3, 6 ), 0 );
ASSERT_EQ( layers.getCostmap( )->getCost( 3, 7 ), 254 );
}
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <vector>
#include <memory>
#include <chrono>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/layered_costmap.hpp"
#include "nav2_costmap_2d/static_layer.hpp"
#include "nav2_costmap_2d/inflation_layer.hpp"
#include "nav2_costmap_2d/costmap_2d_publisher.hpp"
#include "../testing_helper.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav2_util/node_utils.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/create_timer_ros.h"
#include "tf2_ros/transform_broadcaster.h"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpedantic"
#include "tf2/utils.h"
#pragma GCC diagnostic pop
#include "nav2_util/geometry_utils.hpp"
using namespace std::chrono_literals;
using namespace std::placeholders;
using nav2_util::geometry_utils::orientationAroundZAxis;
46 class RclCppFixture
{
public:
49 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
50 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
54 class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber
{
public:
57 DummyCostmapSubscriber(
58 nav2_util::LifecycleNode::SharedPtr node,
59 std::string & topic_name )
: CostmapSubscriber( node, topic_name )
{}
63 void setCostmap( nav2_msgs::msg::Costmap::SharedPtr msg )
{
costmap_msg_ = msg;
costmap_received_ = true;
}
};
70 class DummyFootprintSubscriber : public nav2_costmap_2d::FootprintSubscriber
{
public:
73 DummyFootprintSubscriber(
74 nav2_util::LifecycleNode::SharedPtr node,
75 std::string & topic_name,
76 tf2_ros::Buffer & tf_ )
: FootprintSubscriber( node, topic_name, tf_ )
{}
80 void setFootprint( geometry_msgs::msg::PolygonStamped::SharedPtr msg )
{
footprint_ = msg;
footprint_received_ = true;
}
};
87 class TestCollisionChecker : public nav2_util::LifecycleNode
{
public:
90 explicit TestCollisionChecker( std::string name )
: LifecycleNode( name ),
global_frame_( "map" )
{
// Declare non-plugin specific costmap parameters
declare_parameter( "map_topic", rclcpp::ParameterValue( std::string( "map" ) ) );
declare_parameter( "track_unknown_space", rclcpp::ParameterValue( true ) );
declare_parameter( "use_maximum", rclcpp::ParameterValue( false ) );
declare_parameter( "lethal_cost_threshold", rclcpp::ParameterValue( 100 ) );
declare_parameter(
"unknown_cost_value",
rclcpp::ParameterValue( static_cast<unsigned char>( 0xff ) ) );
declare_parameter( "trinary_costmap", rclcpp::ParameterValue( true ) );
}
nav2_util::CallbackReturn
106 on_configure( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Configuring" );
callback_group_ = create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive, false );
tf_buffer_ = std::make_shared<tf2_ros::Buffer>( get_clock( ) );
auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
get_node_base_interface( ),
get_node_timers_interface( ),
callback_group_ );
tf_buffer_->setCreateTimerInterface( timer_interface );
tf_listener_ = std::make_shared<tf2_ros::TransformListener>( *tf_buffer_ );
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>( shared_from_this( ) );
std::string costmap_topic = "costmap_raw";
std::string footprint_topic = "published_footprint";
costmap_sub_ = std::make_shared<DummyCostmapSubscriber>(
shared_from_this( ),
costmap_topic );
footprint_sub_ = std::make_shared<DummyFootprintSubscriber>(
shared_from_this( ),
footprint_topic,
*tf_buffer_ );
collision_checker_ = std::make_unique<nav2_costmap_2d::CostmapTopicCollisionChecker>(
*costmap_sub_, *footprint_sub_, get_name( ) );
layers_ = new nav2_costmap_2d::LayeredCostmap( "map", false, false );
// Add Static Layer
std::shared_ptr<nav2_costmap_2d::StaticLayer> slayer = nullptr;
addStaticLayer( *layers_, *tf_buffer_, shared_from_this( ), slayer, callback_group_ );
while ( !slayer->isCurrent( ) ) {
rclcpp::spin_some( this->get_node_base_interface( ) );
}
// Add Inflation Layer
std::shared_ptr<nav2_costmap_2d::InflationLayer> ilayer = nullptr;
addInflationLayer( *layers_, *tf_buffer_, shared_from_this( ), ilayer, callback_group_ );
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>( );
executor_->add_callback_group( callback_group_, get_node_base_interface( ) );
executor_thread_ = std::make_unique<nav2_util::NodeThread>( executor_ );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
154 on_activate( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Activating" );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
161 on_deactivate( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Deactivating" );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
168 on_cleanup( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Cleaning Up" );
delete layers_;
layers_ = nullptr;
executor_thread_.reset( );
tf_buffer_.reset( );
footprint_sub_.reset( );
costmap_sub_.reset( );
return nav2_util::CallbackReturn::SUCCESS;
}
183 ~TestCollisionChecker( ) {}
185 bool testPose( double x, double y, double theta )
{
rclcpp::Time stamp = now( );
publishPose( x, y, theta, stamp );
geometry_msgs::msg::Pose2D pose;
pose.x = x;
pose.y = y;
pose.theta = theta;
setPose( x, y, theta, stamp );
publishFootprint( );
publishCostmap( );
rclcpp::sleep_for( std::chrono::milliseconds( 1000 ) );
return collision_checker_->isCollisionFree( pose );
}
201 void setFootprint( double footprint_padding, double robot_radius )
{
std::vector<geometry_msgs::msg::Point> new_footprint;
new_footprint = nav2_costmap_2d::makeFootprintFromRadius( robot_radius );
nav2_costmap_2d::padFootprint( new_footprint, footprint_padding );
footprint_ = new_footprint;
layers_->setFootprint( footprint_ );
}
protected:
211 void setPose( double x, double y, double theta, const rclcpp::Time & stamp )
{
x_ = x;
y_ = y;
yaw_ = theta;
stamp_ = stamp;
current_pose_.pose.position.x = x_;
current_pose_.pose.position.y = y_;
current_pose_.pose.position.z = 0;
current_pose_.pose.orientation = orientationAroundZAxis( yaw_ );
current_pose_.header.stamp = stamp;
}
225 void publishFootprint( )
{
geometry_msgs::msg::PolygonStamped oriented_footprint;
oriented_footprint.header.frame_id = global_frame_;
oriented_footprint.header.stamp = stamp_;
nav2_costmap_2d::transformFootprint( x_, y_, yaw_, footprint_, oriented_footprint );
footprint_sub_->setFootprint(
std::make_shared<geometry_msgs::msg::PolygonStamped>( oriented_footprint ) );
}
235 void publishCostmap( )
{
layers_->updateMap( x_, y_, yaw_ );
costmap_sub_->setCostmap(
std::make_shared<nav2_msgs::msg::Costmap>( toCostmapMsg( layers_->getCostmap( ) ) ) );
}
242 void publishPose( double x, double y, double /*theta*/, const rclcpp::Time & stamp )
{
geometry_msgs::msg::TransformStamped tf_stamped;
tf_stamped.header.frame_id = "map";
tf_stamped.header.stamp = stamp;
tf_stamped.child_frame_id = "base_link";
tf_stamped.transform.translation.x = x;
tf_stamped.transform.translation.y = y;
tf_stamped.transform.rotation.w = 1.0;
tf_broadcaster_->sendTransform( tf_stamped );
}
nav2_msgs::msg::Costmap
255 toCostmapMsg( nav2_costmap_2d::Costmap2D * costmap )
{
double resolution = costmap->getResolution( );
double wx, wy;
costmap->mapToWorld( 0, 0, wx, wy );
unsigned char * data = costmap->getCharMap( );
nav2_msgs::msg::Costmap costmap_msg;
costmap_msg.header.frame_id = global_frame_;
costmap_msg.header.stamp = stamp_;
costmap_msg.metadata.layer = "master";
costmap_msg.metadata.resolution = resolution;
costmap_msg.metadata.size_x = costmap->getSizeInCellsX( );
costmap_msg.metadata.size_y = costmap->getSizeInCellsY( );
costmap_msg.metadata.origin.position.x = wx - resolution / 2;
costmap_msg.metadata.origin.position.y = wy - resolution / 2;
costmap_msg.metadata.origin.position.z = 0.0;
costmap_msg.metadata.origin.orientation.w = 1.0;
costmap_msg.data.resize( costmap_msg.metadata.size_x * costmap_msg.metadata.size_y );
for ( unsigned int i = 0; i < costmap_msg.data.size( ); i++ ) {
costmap_msg.data[i] = data[i];
}
return costmap_msg;
}
284 std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
285 std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
286 std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
288 rclcpp::CallbackGroup::SharedPtr callback_group_;
289 rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
290 std::unique_ptr<nav2_util::NodeThread> executor_thread_;
292 std::shared_ptr<DummyCostmapSubscriber> costmap_sub_;
293 std::shared_ptr<DummyFootprintSubscriber> footprint_sub_;
294 std::unique_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
nav2_costmap_2d::LayeredCostmap * layers_{nullptr};
std::string global_frame_;
double x_, y_, yaw_;
rclcpp::Time stamp_;
geometry_msgs::msg::PoseStamped current_pose_;
std::vector<geometry_msgs::msg::Point> footprint_;
};
class TestNode : public ::testing::Test
{
public:
TestNode( )
{
collision_checker_ = std::make_shared<TestCollisionChecker>( "test_collision_checker" );
collision_checker_->on_configure( collision_checker_->get_current_state( ) );
collision_checker_->on_activate( collision_checker_->get_current_state( ) );
}
~TestNode( )
{
collision_checker_->on_deactivate( collision_checker_->get_current_state( ) );
collision_checker_->on_cleanup( collision_checker_->get_current_state( ) );
}
protected:
std::shared_ptr<TestCollisionChecker> collision_checker_;
};
TEST_F( TestNode, unknownSpace )
{
collision_checker_->setFootprint( 0, 1 );
// Completely off map
ASSERT_EQ( collision_checker_->testPose( 5, 13, 0 ), false );
// Partially off map
ASSERT_EQ( collision_checker_->testPose( 5, 9.5, 0 ), false );
// In unknown region inside map
ASSERT_EQ( collision_checker_->testPose( 2, 4, 0 ), false );
}
TEST_F( TestNode, FreeSpace )
{
collision_checker_->setFootprint( 0, 1 );
// In complete free space
ASSERT_EQ( collision_checker_->testPose( 2, 8.5, 0 ), true );
// Partially in inscribed space
ASSERT_EQ( collision_checker_->testPose( 2.5, 7, 0 ), true );
}
TEST_F( TestNode, CollisionSpace )
{
collision_checker_->setFootprint( 0, 1 );
// Completely in obstacle
ASSERT_EQ( collision_checker_->testPose( 8.5, 6.5, 0 ), false );
// Partially in obstacle
ASSERT_EQ( collision_checker_->testPose( 4.5, 4.5, 0 ), false );
}
1 /*
* Copyright ( c ) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES ( INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE )
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @author Conor McGann
* Test harness for Costmap2D
*/
#include <gtest/gtest.h>
#include <set>
#include <vector>
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/observation_buffer.hpp"
const unsigned char MAP_10_BY_10_CHAR[] = {
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 200, 200, 200,
0, 0, 0, 0, 100, 0, 0, 200, 200, 200,
0, 0, 0, 0, 100, 0, 0, 200, 200, 200,
70, 70, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 200, 200, 200, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 255, 255, 255,
0, 0, 0, 0, 0, 0, 0, 255, 255, 255
};
const unsigned char MAP_5_BY_5_CHAR[] = {
0, 0, 0, 0, 0,
0, 0, 0, 0, 0,
0, 0, 0, 0, 0,
0, 0, 0, 0, 0,
0, 0, 0, 0, 0,
};
std::vector<unsigned char> MAP_5_BY_5;
std::vector<unsigned char> MAP_10_BY_10;
std::vector<unsigned char> EMPTY_10_BY_10;
std::vector<unsigned char> EMPTY_100_BY_100;
67 const unsigned int GRID_WIDTH( 10 );
68 const unsigned int GRID_HEIGHT( 10 );
69 const double RESOLUTION( 1 );
70 const double WINDOW_LENGTH( 10 );
71 const unsigned char THRESHOLD( 100 );
72 const double MAX_Z( 1.0 );
73 const double RAYTRACE_MAX_RANGE( 20.0 );
74 const double RAYTRACE_MIN_RANGE( 3.0 );
75 const double OBSTACLE_MAX_RANGE( 20.0 );
76 const double OBSTACLE_MIN_RANGE( 0.0 );
77 const double ROBOT_RADIUS( 1.0 );
79 bool find( const std::vector<unsigned int> & l, unsigned int n )
{
for ( std::vector<unsigned int>::const_iterator it = l.begin( ); it != l.end( ); ++it ) {
if ( *it == n ) {
return true;
}
}
return false;
}
/**
* Tests the reset method
*/
93 TEST( costmap, testResetForStaticMap ) {
// Define a static map with a large object in the center
std::vector<unsigned char> staticMap;
for ( unsigned int i = 0; i < 10; i++ ) {
for ( unsigned int j = 0; j < 10; j++ ) {
staticMap.push_back( nav2_costmap_2d::LETHAL_OBSTACLE );
}
}
// Allocate the cost map, with a inflation to 3 cells all around
nav2_costmap_2d::Costmap2D map( 10, 10, RESOLUTION, 0.0, 0.0, 3, 3, 3,
OBSTACLE_MAX_RANGE, OBSTACLE_MIN_RANGE, MAX_Z, RAYTRACE_MAX_RANGE, RAYTRACE_MIN_RANGE, 25,
staticMap, THRESHOLD );
// Populate the cost map with a wall around the perimeter. Free space should clear out the room.
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.points.resize( 40 );
// Left wall
unsigned int ind = 0;
for ( unsigned int i = 0; i < 10; i++ ) {
// Left
cloud.points[ind].x = 0;
cloud.points[ind].y = i;
cloud.points[ind].z = MAX_Z;
ind++;
// Top
cloud.points[ind].x = i;
cloud.points[ind].y = 0;
cloud.points[ind].z = MAX_Z;
ind++;
// Right
cloud.points[ind].x = 9;
cloud.points[ind].y = i;
cloud.points[ind].z = MAX_Z;
ind++;
// Bottom
cloud.points[ind].x = i;
cloud.points[ind].y = 9;
cloud.points[ind].z = MAX_Z;
ind++;
}
double wx = 5.0, wy = 5.0;
geometry_msgs::Point p;
p.x = wx;
p.y = wy;
p.z = MAX_Z;
nav2_costmap_2d::Observation obs( p, cloud, OBSTACLE_MAX_RANGE, OBSTACLE_MIN_RANGE,
RAYTRACE_MAX_RANGE,
RAYTRACE_MIN_RANGE );
std::vector<nav2_costmap_2d::Observation> obsBuf;
obsBuf.push_back( obs );
// Update the cost map for this observation
map.updateWorld( wx, wy, obsBuf, obsBuf );
// Verify that we now have only 36 cells with lethal cost,
// thus provong that we have correctly cleared free space
int hitCount = 0;
for ( unsigned int i = 0; i < 10; ++i ) {
for ( unsigned int j = 0; j < 10; ++j ) {
if ( map.getCost( i, j ) == nav2_costmap_2d::LETHAL_OBSTACLE ) {
hitCount++;
}
}
}
ASSERT_EQ( hitCount, 36 );
// Veriy that we have 64 non-leathal
hitCount = 0;
for ( unsigned int i = 0; i < 10; ++i ) {
for ( unsigned int j = 0; j < 10; ++j ) {
if ( map.getCost( i, j ) != nav2_costmap_2d::LETHAL_OBSTACLE ) {
hitCount++;
}
}
}
ASSERT_EQ( hitCount, 64 );
// Now if we reset the cost map, we should have our map go back to being completely occupied
map.resetMapOutsideWindow( wx, wy, 0.0, 0.0 );
// We should now go back to everything being occupied
hitCount = 0;
for ( unsigned int i = 0; i < 10; ++i ) {
for ( unsigned int j = 0; j < 10; ++j ) {
if ( map.getCost( i, j ) == nav2_costmap_2d::LETHAL_OBSTACLE ) {
hitCount++;
}
}
}
ASSERT_EQ( hitCount, 100 );
}
/**
* Test for the cost function correctness with a larger range and different values
*/
194 TEST( costmap, testCostFunctionCorrectness ) {
nav2_costmap_2d::Costmap2D map( 100, 100, RESOLUTION, 0.0, 0.0,
ROBOT_RADIUS * 5.0, ROBOT_RADIUS * 8.0, ROBOT_RADIUS * 10.5,
100.0, MAX_Z, 100.0, 25, EMPTY_100_BY_100, THRESHOLD );
// Verify that the circumscribed cost lower bound is as expected: based on the cost function.
unsigned char c = map.computeCost( ( ROBOT_RADIUS * 8.0 / RESOLUTION ) );
ASSERT_EQ( map.getCircumscribedCost( ), c );
// Add a point in the center
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.points.resize( 1 );
cloud.points[0].x = 50;
cloud.points[0].y = 50;
cloud.points[0].z = MAX_Z;
geometry_msgs::Point p;
p.x = 0.0;
p.y = 0.0;
p.z = MAX_Z;
nav2_costmap_2d::Observation obs( p, cloud, 100.0, 100.0 );
std::vector<nav2_costmap_2d::Observation> obsBuf;
obsBuf.push_back( obs );
map.updateWorld( 0, 0, obsBuf, obsBuf );
for ( unsigned int i = 0; i <= ( unsigned int )ceil( ROBOT_RADIUS * 5.0 ); i++ ) {
// To the right
ASSERT_EQ( map.getCost( 50 + i, 50 ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true );
ASSERT_EQ( map.getCost( 50 + i, 50 ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true );
// To the left
ASSERT_EQ( map.getCost( 50 - i, 50 ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true );
ASSERT_EQ( map.getCost( 50 - i, 50 ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true );
// Down
ASSERT_EQ( map.getCost( 50, 50 + i ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true );
ASSERT_EQ( map.getCost( 50, 50 + i ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true );
// Up
ASSERT_EQ( map.getCost( 50, 50 - i ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true );
ASSERT_EQ( map.getCost( 50, 50 - i ) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true );
}
// Verify the normalized cost attenuates as expected
for ( unsigned int i = ( unsigned int )( ceil( ROBOT_RADIUS * 5.0 ) + 1 );
i <= ( unsigned int )ceil( ROBOT_RADIUS * 10.5 ); i++ )
{
unsigned char expectedValue = map.computeCost( i / RESOLUTION );
ASSERT_EQ( map.getCost( 50 + i, 50 ), expectedValue );
}
// Update with no hits. Should clear ( revert to the static map
map.resetMapOutsideWindow( 0, 0, 0.0, 0.0 );
cloud.points.resize( 0 );
p.x = 0.0;
p.y = 0.0;
p.z = MAX_Z;
nav2_costmap_2d::Observation obs2( p, cloud, 100.0, 100.0 );
std::vector<nav2_costmap_2d::Observation> obsBuf2;
obsBuf2.push_back( obs2 );
map.updateWorld( 0, 0, obsBuf2, obsBuf2 );
for ( unsigned int i = 0; i < 100; i++ ) {
for ( unsigned int j = 0; j < 100; j++ ) {
ASSERT_EQ( map.getCost( i, j ), nav2_costmap_2d::FREE_SPACE );
}
}
}
265 char printableCost( unsigned char cost )
{
switch ( cost ) {
case nav2_costmap_2d::NO_INFORMATION: return '?';
case nav2_costmap_2d::LETHAL_OBSTACLE: return 'L';
case nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE: return 'I';
case nav2_costmap_2d::FREE_SPACE: return '.';
default: return '0' + ( unsigned char ) ( 10 * cost / 255 );
}
}
/**
* Test for wave interference
*/
279 TEST( costmap, testWaveInterference ) {
// Start with an empty map
nav2_costmap_2d::Costmap2D map( GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0,
ROBOT_RADIUS, ROBOT_RADIUS * 2, ROBOT_RADIUS * 3.01,
10.0, MAX_Z * 2, 10.0, 1, EMPTY_10_BY_10, THRESHOLD );
// Lay out 3 obstacles in a line - along the diagonal, separated by a cell.
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.points.resize( 3 );
cloud.points[0].x = 3;
cloud.points[0].y = 3;
cloud.points[0].z = MAX_Z;
cloud.points[1].x = 5;
cloud.points[1].y = 5;
cloud.points[1].z = MAX_Z;
cloud.points[2].x = 7;
cloud.points[2].y = 7;
cloud.points[2].z = MAX_Z;
geometry_msgs::Point p;
p.x = 0.0;
p.y = 0.0;
p.z = MAX_Z;
nav2_costmap_2d::Observation obs( p, cloud, 100.0, 100.0 );
std::vector<nav2_costmap_2d::Observation> obsBuf;
obsBuf.push_back( obs );
map.updateWorld( 0, 0, obsBuf, obsBuf );
int update_count = 0;
// Expect to see a union of obstacles
printf( "map:\n" );
for ( unsigned int i = 0; i < 10; ++i ) {
for ( unsigned int j = 0; j < 10; ++j ) {
if ( map.getCost( i, j ) != nav2_costmap_2d::FREE_SPACE ) {
update_count++;
}
printf( "%c", printableCost( map.getCost( i, j ) ) );
}
printf( "\n" );
}
ASSERT_EQ( update_count, 79 );
}
/** Test for copying a window of a costmap */
327 TEST( costmap, testWindowCopy ) {
nav2_costmap_2d::Costmap2D map( GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0,
ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD );
/*
for( unsigned int i = 0; i < 10; ++i ){
for( unsigned int j = 0; j < 10; ++j ){
printf( "%3d ", map.getCost( i, j ) );
}
printf( "\n" );
}
printf( "\n" );
*/
nav2_costmap_2d::Costmap2D windowCopy;
// first test that if we try to make a window that is too big, things fail
windowCopy.copyCostmapWindow( map, 2.0, 2.0, 6.0, 12.0 );
ASSERT_EQ( windowCopy.getSizeInCellsX( ), ( unsigned int )0 );
ASSERT_EQ( windowCopy.getSizeInCellsY( ), ( unsigned int )0 );
// Next, test that trying to make a map window itself fails
map.copyCostmapWindow( map, 2.0, 2.0, 6.0, 6.0 );
ASSERT_EQ( map.getSizeInCellsX( ), ( unsigned int )10 );
ASSERT_EQ( map.getSizeInCellsY( ), ( unsigned int )10 );
// Next, test that legal input generates the result that we expect
windowCopy.copyCostmapWindow( map, 2.0, 2.0, 6.0, 6.0 );
ASSERT_EQ( windowCopy.getSizeInCellsX( ), ( unsigned int )6 );
ASSERT_EQ( windowCopy.getSizeInCellsY( ), ( unsigned int )6 );
// check that we actually get the windo that we expect
for ( unsigned int i = 0; i < windowCopy.getSizeInCellsX( ); ++i ) {
for ( unsigned int j = 0; j < windowCopy.getSizeInCellsY( ); ++j ) {
ASSERT_EQ( windowCopy.getCost( i, j ), map.getCost( i + 2, j + 2 ) );
// printf( "%3d ", windowCopy.getCost( i, j ) );
}
// printf( "\n" );
}
}
// test for updating costmaps with static data
370 TEST( costmap, testFullyContainedStaticMapUpdate ) {
nav2_costmap_2d::Costmap2D map( 5, 5, RESOLUTION, 0.0, 0.0,
ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
10.0, MAX_Z, 10.0, 25, MAP_5_BY_5, THRESHOLD );
nav2_costmap_2d::Costmap2D static_map( GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0,
ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD );
map.updateStaticMapWindow( 0, 0, 10, 10, MAP_10_BY_10 );
for ( unsigned int i = 0; i < map.getSizeInCellsX( ); ++i ) {
for ( unsigned int j = 0; j < map.getSizeInCellsY( ); ++j ) {
ASSERT_EQ( map.getCost( i, j ), static_map.getCost( i, j ) );
}
}
}
388 TEST( costmap, testOverlapStaticMapUpdate ) {
nav2_costmap_2d::Costmap2D map( 5, 5, RESOLUTION, 0.0, 0.0,
ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
10.0, MAX_Z, 10.0, 25, MAP_5_BY_5, THRESHOLD );
nav2_costmap_2d::Costmap2D static_map( GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0,
ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD );
map.updateStaticMapWindow( -10, -10, 10, 10, MAP_10_BY_10 );
ASSERT_FLOAT_EQ( map.getOriginX( ), -10 );
ASSERT_FLOAT_EQ( map.getOriginX( ), -10 );
ASSERT_EQ( map.getSizeInCellsX( ), ( unsigned int )15 );
ASSERT_EQ( map.getSizeInCellsY( ), ( unsigned int )15 );
for ( unsigned int i = 0; i < 10; ++i ) {
for ( unsigned int j = 0; j < 10; ++j ) {
ASSERT_EQ( map.getCost( i, j ), static_map.getCost( i, j ) );
}
}
std::vector<unsigned char> blank( 100 );
// check to make sure that inflation on updates are being done correctly
map.updateStaticMapWindow( -10, -10, 10, 10, blank );
for ( unsigned int i = 0; i < map.getSizeInCellsX( ); ++i ) {
for ( unsigned int j = 0; j < map.getSizeInCellsY( ); ++j ) {
ASSERT_EQ( map.getCost( i, j ), 0 );
}
}
std::vector<unsigned char> fully_contained( 25 );
fully_contained[0] = 254;
fully_contained[4] = 254;
fully_contained[5] = 254;
fully_contained[9] = 254;
nav2_costmap_2d::Costmap2D small_static_map( 5, 5, RESOLUTION, 0.0, 0.0,
ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
10.0, MAX_Z, 10.0, 25, fully_contained, THRESHOLD );
map.updateStaticMapWindow( 0, 0, 5, 5, fully_contained );
ASSERT_FLOAT_EQ( map.getOriginX( ), -10 );
ASSERT_FLOAT_EQ( map.getOriginX( ), -10 );
ASSERT_EQ( map.getSizeInCellsX( ), ( unsigned int )15 );
ASSERT_EQ( map.getSizeInCellsY( ), ( unsigned int )15 );
for ( unsigned int j = 0; j < 5; ++j ) {
for ( unsigned int i = 0; i < 5; ++i ) {
ASSERT_EQ( map.getCost( i + 10, j + 10 ), small_static_map.getCost( i, j ) );
}
}
}
/**
* Test for ray tracing free space
*/
446 TEST( costmap, testRaytracing ) {
nav2_costmap_2d::Costmap2D map( GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0,
ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD );
// Add a point cloud, should not affect the map
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.points.resize( 1 );
cloud.points[0].x = 0;
cloud.points[0].y = 0;
cloud.points[0].z = MAX_Z;
geometry_msgs::Point p;
p.x = 0.0;
p.y = 0.0;
p.z = MAX_Z;
nav2_costmap_2d::Observation obs( p, cloud, 100.0, 100.0 );
std::vector<nav2_costmap_2d::Observation> obsBuf;
obsBuf.push_back( obs );
map.updateWorld( 0, 0, obsBuf, obsBuf );
int lethal_count = 0;
for ( unsigned int i = 0; i < 10; ++i ) {
for ( unsigned int j = 0; j < 10; ++j ) {
if ( map.getCost( i, j ) == nav2_costmap_2d::LETHAL_OBSTACLE ) {
lethal_count++;
}
}
}
// we expect just one obstacle to be added
ASSERT_EQ( lethal_count, 21 );
}
483 TEST( costmap, testAdjacentToObstacleCanStillMove ) {
nav2_costmap_2d::Costmap2D map( GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, 2.1, 3.1, 4.1,
10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD );
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.points.resize( 1 );
cloud.points[0].x = 0;
cloud.points[0].y = 0;
cloud.points[0].z = MAX_Z;
geometry_msgs::Point p;
p.x = 0.0;
p.y = 0.0;
p.z = MAX_Z;
nav2_costmap_2d::Observation obs( p, cloud, 100.0, 100.0 );
std::vector<nav2_costmap_2d::Observation> obsBuf;
obsBuf.push_back( obs );
map.updateWorld( 9, 9, obsBuf, obsBuf );
EXPECT_EQ( nav2_costmap_2d::LETHAL_OBSTACLE, map.getCost( 0, 0 ) );
EXPECT_EQ( nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, map.getCost( 1, 0 ) );
EXPECT_EQ( nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, map.getCost( 2, 0 ) );
EXPECT_TRUE( nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE > map.getCost( 3, 0 ) );
EXPECT_TRUE( nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE > map.getCost( 2, 1 ) );
EXPECT_EQ( nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, map.getCost( 1, 1 ) );
}
511 TEST( costmap, testInflationShouldNotCreateUnknowns ) {
nav2_costmap_2d::Costmap2D map( GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, 2.1, 3.1, 4.1,
10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD );
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.points.resize( 1 );
cloud.points[0].x = 0;
cloud.points[0].y = 0;
cloud.points[0].z = MAX_Z;
geometry_msgs::Point p;
p.x = 0.0;
p.y = 0.0;
p.z = MAX_Z;
nav2_costmap_2d::Observation obs( p, cloud, 100.0, 100.0 );
std::vector<nav2_costmap_2d::Observation> obsBuf;
obsBuf.push_back( obs );
map.updateWorld( 9, 9, obsBuf, obsBuf );
int unknown_count = 0;
for ( unsigned int i = 0; i < 10; ++i ) {
for ( unsigned int j = 0; j < 10; ++j ) {
if ( map.getCost( i, j ) == nav2_costmap_2d::NO_INFORMATION ) {
unknown_count++;
}
}
}
EXPECT_EQ( 0, unknown_count );
}
543 unsigned int worldToIndex( nav2_costmap_2d::Costmap2D & map, double wx, double wy )
{
unsigned int mx, my;
map.worldToMap( wx, wy, mx, my );
return map.getIndex( mx, my );
}
550 void indexToWorld( nav2_costmap_2d::Costmap2D & map, unsigned int index, double & wx, double & wy )
{
unsigned int mx, my;
map.indexToCells( index, mx, my );
map.mapToWorld( mx, my, wx, wy );
}
557 TEST( costmap, testStaticMap ) {
nav2_costmap_2d::Costmap2D map( GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0,
ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD );
ASSERT_EQ( map.getSizeInCellsX( ), ( unsigned int )10 );
ASSERT_EQ( map.getSizeInCellsY( ), ( unsigned int )10 );
// Verify that obstacles correctly identified from the static map.
std::vector<unsigned int> occupiedCells;
for ( unsigned int i = 0; i < 10; ++i ) {
for ( unsigned int j = 0; j < 10; ++j ) {
if ( map.getCost( i, j ) == nav2_costmap_2d::LETHAL_OBSTACLE ) {
occupiedCells.push_back( map.getIndex( i, j ) );
}
}
}
ASSERT_EQ( occupiedCells.size( ), ( unsigned int )20 );
// Iterate over all id's and verify that they are present according to their
for ( std::vector<unsigned int>::const_iterator it = occupiedCells.begin( );
it != occupiedCells.end( ); ++it )
{
unsigned int ind = *it;
unsigned int x, y;
map.indexToCells( ind, x, y );
ASSERT_EQ( find( occupiedCells, map.getIndex( x, y ) ), true );
ASSERT_EQ( MAP_10_BY_10[ind] >= 100, true );
ASSERT_EQ( map.getCost( x, y ) >= 100, true );
}
// Block of 200
ASSERT_EQ( find( occupiedCells, map.getIndex( 7, 2 ) ), true );
ASSERT_EQ( find( occupiedCells, map.getIndex( 8, 2 ) ), true );
ASSERT_EQ( find( occupiedCells, map.getIndex( 9, 2 ) ), true );
ASSERT_EQ( find( occupiedCells, map.getIndex( 7, 3 ) ), true );
ASSERT_EQ( find( occupiedCells, map.getIndex( 8, 3 ) ), true );
ASSERT_EQ( find( occupiedCells, map.getIndex( 9, 3 ) ), true );
ASSERT_EQ( find( occupiedCells, map.getIndex( 7, 4 ) ), true );
ASSERT_EQ( find( occupiedCells, map.getIndex( 8, 4 ) ), true );
ASSERT_EQ( find( occupiedCells, map.getIndex( 9, 4 ) ), true );
// Block of 100
ASSERT_EQ( find( occupiedCells, map.getIndex( 4, 3 ) ), true );
ASSERT_EQ( find( occupiedCells, map.getIndex( 4, 4 ) ), true );
// Block of 200
ASSERT_EQ( find( occupiedCells, map.getIndex( 3, 7 ) ), true );
ASSERT_EQ( find( occupiedCells, map.getIndex( 4, 7 ) ), true );
ASSERT_EQ( find( occupiedCells, map.getIndex( 5, 7 ) ), true );
// Verify Coordinate Transformations, ROW MAJOR ORDER
ASSERT_EQ( worldToIndex( map, 0.0, 0.0 ), ( unsigned int )0 );
ASSERT_EQ( worldToIndex( map, 0.0, 0.99 ), ( unsigned int )0 );
ASSERT_EQ( worldToIndex( map, 0.0, 1.0 ), ( unsigned int )10 );
ASSERT_EQ( worldToIndex( map, 1.0, 0.99 ), ( unsigned int )1 );
ASSERT_EQ( worldToIndex( map, 9.99, 9.99 ), ( unsigned int )99 );
ASSERT_EQ( worldToIndex( map, 8.2, 3.4 ), ( unsigned int )38 );
// Ensure we hit the middle of the cell for world co-ordinates
double wx, wy;
indexToWorld( map, 99, wx, wy );
ASSERT_EQ( wx, 9.5 );
ASSERT_EQ( wy, 9.5 );
}
/**
* Verify that dynamic obstacles are added
*/
631 TEST( costmap, testDynamicObstacles ) {
nav2_costmap_2d::Costmap2D map( GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0,
ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD );
// Add a point cloud and verify its insertion. There should be only one new one
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.points.resize( 3 );
cloud.points[0].x = 0;
cloud.points[0].y = 0;
cloud.points[1].x = 0;
cloud.points[1].y = 0;
cloud.points[2].x = 0;
cloud.points[2].y = 0;
geometry_msgs::Point p;
p.x = 0.0;
p.y = 0.0;
p.z = MAX_Z;
nav2_costmap_2d::Observation obs( p, cloud, 100.0, 100.0 );
std::vector<nav2_costmap_2d::Observation> obsBuf;
obsBuf.push_back( obs );
map.updateWorld( 0, 0, obsBuf, obsBuf );
std::vector<unsigned int> ids;
for ( unsigned int i = 0; i < 10; ++i ) {
for ( unsigned int j = 0; j < 10; ++j ) {
if ( map.getCost( i, j ) == nav2_costmap_2d::LETHAL_OBSTACLE ) {
ids.push_back( map.getIndex( i, j ) );
}
}
}
// Should now have 1 insertion and no deletions
ASSERT_EQ( ids.size( ), ( unsigned int )21 );
// Repeating the call - we should see no insertions or deletions
map.updateWorld( 0, 0, obsBuf, obsBuf );
ASSERT_EQ( ids.size( ), ( unsigned int )21 );
}
/**
* Verify that if we add a point that is already a static obstacle we do not end up with a new ostacle
*/
678 TEST( costmap, testMultipleAdditions ) {
nav2_costmap_2d::Costmap2D map( GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0,
ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD );
// A point cloud with one point that falls within an existing obstacle
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.points.resize( 1 );
cloud.points[0].x = 7;
cloud.points[0].y = 2;
geometry_msgs::Point p;
p.x = 0.0;
p.y = 0.0;
p.z = MAX_Z;
nav2_costmap_2d::Observation obs( p, cloud, 100.0, 100.0 );
std::vector<nav2_costmap_2d::Observation> obsBuf;
obsBuf.push_back( obs );
map.updateWorld( 0, 0, obsBuf, obsBuf );
std::vector<unsigned int> ids;
for ( unsigned int i = 0; i < 10; ++i ) {
for ( unsigned int j = 0; j < 10; ++j ) {
if ( map.getCost( i, j ) == nav2_costmap_2d::LETHAL_OBSTACLE ) {
ids.push_back( map.getIndex( i, j ) );
}
}
}
ASSERT_EQ( ids.size( ), ( unsigned int )20 );
}
/**
* Make sure we ignore points outside of our z threshold
*/
716 TEST( costmap, testZThreshold ) {
nav2_costmap_2d::Costmap2D map( GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0,
ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD );
// A point cloud with 2 points falling in a cell with a non-lethal cost
pcl::PointCloud<pcl::PointXYZ> c0;
c0.points.resize( 2 );
c0.points[0].x = 0;
c0.points[0].y = 5;
c0.points[0].z = 0.4;
c0.points[1].x = 1;
c0.points[1].y = 5;
c0.points[1].z = 1.2;
geometry_msgs::Point p;
p.x = 0.0;
p.y = 0.0;
p.z = MAX_Z;
nav2_costmap_2d::Observation obs( p, c0, 100.0, 100.0 );
std::vector<nav2_costmap_2d::Observation> obsBuf;
obsBuf.push_back( obs );
map.updateWorld( 0, 0, obsBuf, obsBuf );
std::vector<unsigned int> ids;
for ( unsigned int i = 0; i < 10; ++i ) {
for ( unsigned int j = 0; j < 10; ++j ) {
if ( map.getCost( i, j ) == nav2_costmap_2d::LETHAL_OBSTACLE ) {
ids.push_back( map.getIndex( i, j ) );
}
}
}
ASSERT_EQ( ids.size( ), ( unsigned int )21 );
}
/**
* Test inflation for both static and dynamic obstacles
*/
758 TEST( costmap, testInflation ) {
nav2_costmap_2d::Costmap2D map( GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0,
ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD );
// Verify that obstacles correctly identified
std::vector<unsigned int> occupiedCells;
for ( unsigned int i = 0; i < 10; ++i ) {
for ( unsigned int j = 0; j < 10; ++j ) {
if ( map.getCost( i, j ) == nav2_costmap_2d::LETHAL_OBSTACLE ||
map.getCost( i, j ) == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE )
{
occupiedCells.push_back( map.getIndex( i, j ) );
}
}
}
// There should be no duplicates
std::set<unsigned int> setOfCells;
for ( unsigned int i = 0; i < occupiedCells.size( ); i++ ) {
setOfCells.insert( i );
}
ASSERT_EQ( setOfCells.size( ), occupiedCells.size( ) );
ASSERT_EQ( setOfCells.size( ), ( unsigned int )48 );
// Iterate over all id's and verify they are obstacles
for ( std::vector<unsigned int>::const_iterator it = occupiedCells.begin( );
it != occupiedCells.end( ); ++it )
{
unsigned int ind = *it;
unsigned int x, y;
map.indexToCells( ind, x, y );
ASSERT_EQ( find( occupiedCells, map.getIndex( x, y ) ), true );
ASSERT_EQ(
map.getCost( x, y ) == nav2_costmap_2d::LETHAL_OBSTACLE ||
map.getCost( x, y ) == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true );
}
// Set an obstacle at the origin and observe insertions for it and its neighbors
pcl::PointCloud<pcl::PointXYZ> c0;
c0.points.resize( 1 );
c0.points[0].x = 0;
c0.points[0].y = 0;
c0.points[0].z = 0.4;
geometry_msgs::Point p;
p.x = 0.0;
p.y = 0.0;
p.z = MAX_Z;
nav2_costmap_2d::Observation obs( p, c0, 100.0, 100.0 );
std::vector<nav2_costmap_2d::Observation> obsBuf, empty;
obsBuf.push_back( obs );
map.updateWorld( 0, 0, obsBuf, empty );
occupiedCells.clear( );
for ( unsigned int i = 0; i < 10; ++i ) {
for ( unsigned int j = 0; j < 10; ++j ) {
if ( map.getCost( i, j ) == nav2_costmap_2d::LETHAL_OBSTACLE ||
map.getCost( i, j ) == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE )
{
occupiedCells.push_back( map.getIndex( i, j ) );
}
}
}
// It and its 2 neighbors makes 3 obstacles
ASSERT_EQ( occupiedCells.size( ), ( unsigned int )51 );
// @todo Rewrite
// Add an obstacle at <2, 0> which will inflate and refresh to of the other inflated cells
pcl::PointCloud<pcl::PointXYZ> c1;
c1.points.resize( 1 );
c1.points[0].x = 2;
c1.points[0].y = 0;
c1.points[0].z = 0.0;
geometry_msgs::Point p1;
p1.x = 0.0;
p1.y = 0.0;
p1.z = MAX_Z;
nav2_costmap_2d::Observation obs1( p1, c1, 100.0, 100.0 );
std::vector<nav2_costmap_2d::Observation> obsBuf1;
obsBuf1.push_back( obs1 );
map.updateWorld( 0, 0, obsBuf1, empty );
occupiedCells.clear( );
for ( unsigned int i = 0; i < 10; ++i ) {
for ( unsigned int j = 0; j < 10; ++j ) {
if ( map.getCost( i, j ) == nav2_costmap_2d::LETHAL_OBSTACLE ||
map.getCost( i, j ) == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE )
{
occupiedCells.push_back( map.getIndex( i, j ) );
}
}
}
// Now we expect insertions for it, and 2 more neighbors,
// but not all 5. Free space will propagate from
// the origin to the target, clearing the point at <0, 0>,
// but not over-writing the inflation of the obstacle at <0, 1>
ASSERT_EQ( occupiedCells.size( ), ( unsigned int )54 );
// Add an obstacle at <1, 9>. This will inflate obstacles around it
pcl::PointCloud<pcl::PointXYZ> c2;
c2.points.resize( 1 );
c2.points[0].x = 1;
c2.points[0].y = 9;
c2.points[0].z = 0.0;
geometry_msgs::Point p2;
p2.x = 0.0;
p2.y = 0.0;
p2.z = MAX_Z;
nav2_costmap_2d::Observation obs2( p2, c2, 100.0, 100.0 );
std::vector<nav2_costmap_2d::Observation> obsBuf2;
obsBuf2.push_back( obs2 );
map.updateWorld( 0, 0, obsBuf2, empty );
ASSERT_EQ( map.getCost( 1, 9 ), nav2_costmap_2d::LETHAL_OBSTACLE );
ASSERT_EQ( map.getCost( 0, 9 ), nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE );
ASSERT_EQ( map.getCost( 2, 9 ), nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE );
// Add an obstacle and verify that it over-writes its inflated status
pcl::PointCloud<pcl::PointXYZ> c3;
c3.points.resize( 1 );
c3.points[0].x = 0;
c3.points[0].y = 9;
c3.points[0].z = 0.0;
geometry_msgs::Point p3;
p3.x = 0.0;
p3.y = 0.0;
p3.z = MAX_Z;
nav2_costmap_2d::Observation obs3( p3, c3, 100.0, 100.0 );
std::vector<nav2_costmap_2d::Observation> obsBuf3;
obsBuf3.push_back( obs3 );
map.updateWorld( 0, 0, obsBuf3, empty );
ASSERT_EQ( map.getCost( 0, 9 ), nav2_costmap_2d::LETHAL_OBSTACLE );
}
/**
* Test specific inflation scenario to ensure we do not set inflated obstacles to be raw obstacles.
*/
913 TEST( costmap, testInflation2 ) {
nav2_costmap_2d::Costmap2D map( GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0,
ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD );
// Creat a small L-Shape all at once
pcl::PointCloud<pcl::PointXYZ> c0;
c0.points.resize( 3 );
c0.points[0].x = 1;
c0.points[0].y = 1;
c0.points[0].z = MAX_Z;
c0.points[1].x = 1;
c0.points[1].y = 2;
c0.points[1].z = MAX_Z;
c0.points[2].x = 2;
c0.points[2].y = 2;
c0.points[2].z = MAX_Z;
geometry_msgs::Point p;
p.x = 0.0;
p.y = 0.0;
p.z = MAX_Z;
nav2_costmap_2d::Observation obs( p, c0, 100.0, 100.0 );
std::vector<nav2_costmap_2d::Observation> obsBuf;
obsBuf.push_back( obs );
map.updateWorld( 0, 0, obsBuf, obsBuf );
ASSERT_EQ( map.getCost( 3, 2 ), nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE );
ASSERT_EQ( map.getCost( 3, 3 ), nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE );
}
/**
* Test inflation behavior, starting with an empty map
*/
949 TEST( costmap, testInflation3 ) {
std::vector<unsigned char> mapData;
for ( unsigned int i = 0; i < GRID_WIDTH; i++ ) {
for ( unsigned int j = 0; j < GRID_HEIGHT; j++ ) {
mapData.push_back( 0 );
}
}
nav2_costmap_2d::Costmap2D map( GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0,
ROBOT_RADIUS, ROBOT_RADIUS * 2, ROBOT_RADIUS * 3,
10.0, MAX_Z, 10.0, 1, mapData, THRESHOLD );
// There should be no occupied cells
std::vector<unsigned int> ids;
for ( unsigned int i = 0; i < 10; ++i ) {
for ( unsigned int j = 0; j < 10; ++j ) {
if ( map.getCost( i, j ) == nav2_costmap_2d::LETHAL_OBSTACLE ||
map.getCost( i, j ) == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE )
{
ids.push_back( map.getIndex( i, j ) );
}
}
}
ASSERT_EQ( ids.size( ), ( unsigned int )0 );
// Add an obstacle at 5, 5
pcl::PointCloud<pcl::PointXYZ> c0;
c0.points.resize( 1 );
c0.points[0].x = 5;
c0.points[0].y = 5;
c0.points[0].z = MAX_Z;
geometry_msgs::Point p;
p.x = 0.0;
p.y = 0.0;
p.z = MAX_Z;
nav2_costmap_2d::Observation obs( p, c0, 100.0, 100.0 );
std::vector<nav2_costmap_2d::Observation> obsBuf;
obsBuf.push_back( obs );
map.updateWorld( 0, 0, obsBuf, obsBuf );
for ( unsigned int i = 0; i < 10; ++i ) {
for ( unsigned int j = 0; j < 10; ++j ) {
if ( map.getCost( i, j ) != nav2_costmap_2d::FREE_SPACE ) {
ids.push_back( map.getIndex( i, j ) );
}
}
}
ASSERT_EQ( ids.size( ), ( unsigned int )29 );
ids.clear( );
for ( unsigned int i = 0; i < 10; ++i ) {
for ( unsigned int j = 0; j < 10; ++j ) {
if ( map.getCost( i, j ) == nav2_costmap_2d::LETHAL_OBSTACLE ||
map.getCost( i, j ) == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE )
{
ids.push_back( map.getIndex( i, j ) );
}
}
}
ASSERT_EQ( ids.size( ), ( unsigned int )5 );
// Update again - should see no change
map.updateWorld( 0, 0, obsBuf, obsBuf );
ids.clear( );
for ( unsigned int i = 0; i < 10; ++i ) {
for ( unsigned int j = 0; j < 10; ++j ) {
if ( map.getCost( i, j ) != nav2_costmap_2d::FREE_SPACE ) {
ids.push_back( map.getIndex( i, j ) );
}
}
}
ASSERT_EQ( ids.size( ), ( unsigned int )29 );
}
/**
* Test for ray tracing free space
*/
1036 TEST( costmap, testRaytracing2 ) {
nav2_costmap_2d::Costmap2D map( GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0,
ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
100.0, MAX_Z, 100.0, 1, MAP_10_BY_10, THRESHOLD );
// The sensor origin will be <0, 0>. So if we add an obstacle at 9, 9, we would expect cells
// <0, 0> thru <8, 8> to be traced through
pcl::PointCloud<pcl::PointXYZ> c0;
c0.points.resize( 1 );
c0.points[0].x = 9.5;
c0.points[0].y = 9.5;
c0.points[0].z = MAX_Z;
geometry_msgs::Point p;
p.x = 0.5;
p.y = 0.5;
p.z = MAX_Z;
nav2_costmap_2d::Observation obs( p, c0, 100.0, 100.0 );
std::vector<nav2_costmap_2d::Observation> obsBuf;
obsBuf.push_back( obs );
std::vector<unsigned int> obstacles;
for ( unsigned int i = 0; i < 10; ++i ) {
for ( unsigned int j = 0; j < 10; ++j ) {
if ( map.getCost( i, j ) == nav2_costmap_2d::LETHAL_OBSTACLE ) {
obstacles.push_back( map.getIndex( i, j ) );
}
}
}
unsigned int obs_before = obstacles.size( );
map.updateWorld( 0, 0, obsBuf, obsBuf );
obstacles.clear( );
for ( unsigned int i = 0; i < 10; ++i ) {
for ( unsigned int j = 0; j < 10; ++j ) {
if ( map.getCost( i, j ) == nav2_costmap_2d::LETHAL_OBSTACLE ) {
obstacles.push_back( map.getIndex( i, j ) );
}
}
}
// Two obstacles should be removed from the map by raytracing
ASSERT_EQ( obstacles.size( ), obs_before - 2 );
// many cells will have been switched to free space along the diagonal except
// for those inflated in the update.. tests that inflation happens properly
// after raytracing
unsigned char test[10] = {0, 0, 0, 253, 253, 0, 0, 253, 253, 254};
for ( unsigned int i = 0; i < 10; i++ ) {
ASSERT_EQ( map.getCost( i, i ), test[i] );
}
}
/**
* Within a certian radius of the robot, the cost map most propagate obstacles. This
* is to avoid a case where a hit on a far obstacle clears inscribed radius around a
* near one.
*/
1100 TEST( costmap, testTrickyPropagation ) {
const unsigned char MAP_HALL_CHAR[10 * 10] = {
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
254, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 254, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 254, 0, 0, 0, 0,
0, 0, 0, 0, 0, 254, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
};
std::vector<unsigned char> MAP_HALL;
for ( int i = 0; i < 10 * 10; i++ ) {
MAP_HALL.push_back( MAP_HALL_CHAR[i] );
}
nav2_costmap_2d::Costmap2D map( GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0,
ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
100.0, MAX_Z, 100.0, 1, MAP_HALL, THRESHOLD );
// Add a dynamic obstacle
pcl::PointCloud<pcl::PointXYZ> c2;
c2.points.resize( 3 );
// Dynamic obstacle that raytaces.
c2.points[0].x = 7.0;
c2.points[0].y = 8.0;
c2.points[0].z = 1.0;
// Dynamic obstacle that should not be raytraced the
// first update, but should on the second.
c2.points[1].x = 3.0;
c2.points[1].y = 4.0;
c2.points[1].z = 1.0;
// Dynamic obstacle that should not be erased.
c2.points[2].x = 6.0;
c2.points[2].y = 3.0;
c2.points[2].z = 1.0;
geometry_msgs::Point p2;
p2.x = 0.5;
p2.y = 0.5;
p2.z = MAX_Z;
nav2_costmap_2d::Observation obs2( p2, c2, 100.0, 100.0 );
std::vector<nav2_costmap_2d::Observation> obsBuf2;
obsBuf2.push_back( obs2 );
map.updateWorld( 0, 0, obsBuf2, obsBuf2 );
const unsigned char MAP_HALL_CHAR_TEST[10 * 10] = {
253, 254, 253, 0, 0, 0, 0, 0, 0, 0,
0, 253, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 253, 0, 0, 0, 0, 0,
0, 0, 0, 253, 254, 253, 0, 0, 0, 0,
0, 0, 0, 0, 253, 0, 0, 253, 0, 0,
0, 0, 0, 253, 0, 0, 253, 254, 253, 0,
0, 0, 253, 254, 253, 0, 0, 253, 253, 0,
0, 0, 0, 253, 0, 0, 0, 253, 254, 253,
0, 0, 0, 0, 0, 0, 0, 0, 253, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
};
for ( int i = 0; i < 10 * 10; i++ ) {
ASSERT_EQ( map.getCost( i / 10, i % 10 ), MAP_HALL_CHAR_TEST[i] );
}
pcl::PointCloud<pcl::PointXYZ> c;
c.points.resize( 1 );
// Dynamic obstacle that raytaces the one at ( 3.0, 4.0 ).
c.points[0].x = 4.0;
c.points[0].y = 5.0;
c.points[0].z = 1.0;
geometry_msgs::Point p3;
p3.x = 0.5;
p3.y = 0.5;
p3.z = MAX_Z;
nav2_costmap_2d::Observation obs3( p3, c, 100.0, 100.0 );
std::vector<nav2_costmap_2d::Observation> obsBuf3;
obsBuf3.push_back( obs3 );
map.updateWorld( 0, 0, obsBuf3, obsBuf3 );
const unsigned char MAP_HALL_CHAR_TEST2[10 * 10] = {
253, 254, 253, 0, 0, 0, 0, 0, 0, 0,
0, 253, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 253, 0, 0, 0, 0,
0, 0, 0, 0, 253, 254, 253, 253, 0, 0,
0, 0, 0, 253, 0, 253, 253, 254, 253, 0,
0, 0, 253, 254, 253, 0, 0, 253, 253, 0,
0, 0, 0, 253, 0, 0, 0, 253, 254, 253,
0, 0, 0, 0, 0, 0, 0, 0, 253, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
};
for ( int i = 0; i < 10 * 10; i++ ) {
ASSERT_EQ( map.getCost( i / 10, i % 10 ), MAP_HALL_CHAR_TEST2[i] );
}
}
1205 int main( int argc, char ** argv )
{
for ( unsigned int i = 0; i < GRID_WIDTH * GRID_HEIGHT; i++ ) {
EMPTY_10_BY_10.push_back( 0 );
MAP_10_BY_10.push_back( MAP_10_BY_10_CHAR[i] );
}
for ( unsigned int i = 0; i < 5 * 5; i++ ) {
MAP_5_BY_5.push_back( MAP_10_BY_10_CHAR[i] );
}
for ( unsigned int i = 0; i < 100 * 100; i++ ) {
EMPTY_100_BY_100.push_back( 0 );
}
testing::InitGoogleTest( &argc, argv );
return RUN_ALL_TESTS( );
}
1 /*********************************************************************
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2022 Samsung Research Russia
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Alexey Merzlyakov
*********************************************************************/
#include <nav2_costmap_2d/costmap_2d.hpp>
#include <gtest/gtest.h>
39 class CostmapAction
{
public:
42 explicit CostmapAction(
unsigned char * costmap, unsigned int size, unsigned char mark_val = 128 )
: costmap_( costmap ), size_( size ), mark_val_( mark_val )
{
}
48 inline void operator( )( unsigned int off )
{
ASSERT_TRUE( off < size_ );
costmap_[off] = mark_val_;
}
54 inline unsigned int get( unsigned int off )
{
return costmap_[off];
}
private:
unsigned char * costmap_;
unsigned int size_;
unsigned char mark_val_;
};
65 class CostmapTest : public nav2_costmap_2d::Costmap2D
{
public:
68 CostmapTest(
unsigned int size_x, unsigned int size_y, double resolution,
double origin_x, double origin_y, unsigned char default_val = 0 )
: nav2_costmap_2d::Costmap2D( size_x, size_y, resolution, origin_x, origin_y, default_val )
{
}
75 unsigned char * getCostmap( )
{
return costmap_;
}
80 unsigned int getSize( )
{
return size_x_ * size_y_;
}
85 void raytraceLine(
CostmapAction ca, unsigned int x0, unsigned int y0, unsigned int x1,
unsigned int y1,
unsigned int max_length = UINT_MAX, unsigned int min_length = 0 )
{
nav2_costmap_2d::Costmap2D::raytraceLine( ca, x0, y0, x1, y1, max_length, min_length );
}
};
94 TEST( costmap_2d, bresenham2DBoundariesCheck )
{
const unsigned int sz_x = 60;
const unsigned int sz_y = 60;
const unsigned int max_length = 60;
const unsigned int min_length = 6;
CostmapTest ct( sz_x, sz_y, 0.1, 0.0, 0.0 );
CostmapAction ca( ct.getCostmap( ), ct.getSize( ) );
// Initial point - some assymetrically standing point in order to cover most corner cases
const unsigned int x0 = 2;
const unsigned int y0 = 4;
// ( x1, y1 ) point will move
unsigned int x1, y1;
// Running on ( x, 0 ) edge
y1 = 0;
for ( x1 = 0; x1 < sz_x; x1++ ) {
ct.raytraceLine( ca, x0, y0, x1, y1, max_length, min_length );
}
// Running on ( x, sz_y ) edge
y1 = sz_y - 1;
for ( x1 = 0; x1 < sz_x; x1++ ) {
ct.raytraceLine( ca, x0, y0, x1, y1, max_length, min_length );
}
// Running on ( 0, y ) edge
x1 = 0;
for ( y1 = 0; y1 < sz_y; y1++ ) {
ct.raytraceLine( ca, x0, y0, x1, y1, max_length, min_length );
}
// Running on ( sz_x, y ) edge
x1 = sz_x - 1;
for ( y1 = 0; y1 < sz_y; y1++ ) {
ct.raytraceLine( ca, x0, y0, x1, y1, max_length, min_length );
}
}
134 TEST( costmap_2d, bresenham2DSamePoint )
{
const unsigned int sz_x = 60;
const unsigned int sz_y = 60;
const unsigned int max_length = 60;
const unsigned int min_length = 0;
CostmapTest ct( sz_x, sz_y, 0.1, 0.0, 0.0 );
CostmapAction ca( ct.getCostmap( ), ct.getSize( ) );
// Initial point
const double x0 = 2;
const double y0 = 4;
unsigned int offset = y0 * sz_x + x0;
unsigned char val_before = ca.get( offset );
// Same point to check
ct.raytraceLine( ca, x0, y0, x0, y0, max_length, min_length );
unsigned char val_after = ca.get( offset );
ASSERT_FALSE( val_before == val_after );
}
155 int main( int argc, char ** argv )
{
testing::InitGoogleTest( &argc, argv );
return RUN_ALL_TESTS( );
}
1 // Copyright ( c ) Willow Garage
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TESTING_HELPER_HPP_
#define TESTING_HELPER_HPP_
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/point_cloud2_iterator.hpp"
#include "nav2_costmap_2d/cost_values.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/static_layer.hpp"
#include "nav2_costmap_2d/range_sensor_layer.hpp"
#include "nav2_costmap_2d/obstacle_layer.hpp"
#include "nav2_costmap_2d/inflation_layer.hpp"
#include "nav2_util/lifecycle_node.hpp"
31 const double MAX_Z( 1.0 );
33 char printableCost( unsigned char cost )
{
switch ( cost ) {
case nav2_costmap_2d::NO_INFORMATION: return '?';
case nav2_costmap_2d::LETHAL_OBSTACLE: return 'L';
case nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE: return 'I';
case nav2_costmap_2d::FREE_SPACE: return '.';
default: return '0' + ( unsigned char ) ( 10 * cost / 255 );
}
}
44 void printMap( nav2_costmap_2d::Costmap2D & costmap )
{
printf( "map:\n" );
for ( unsigned int i = 0; i < costmap.getSizeInCellsY( ); i++ ) {
for ( unsigned int j = 0; j < costmap.getSizeInCellsX( ); j++ ) {
printf( "%4d", static_cast<int>( costmap.getCost( j, i ) ) );
}
printf( "\n\n" );
}
}
55 unsigned int countValues(
nav2_costmap_2d::Costmap2D & costmap,
unsigned char value, bool equal = true )
{
unsigned int count = 0;
for ( unsigned int i = 0; i < costmap.getSizeInCellsY( ); i++ ) {
for ( unsigned int j = 0; j < costmap.getSizeInCellsX( ); j++ ) {
unsigned char c = costmap.getCost( j, i );
if ( ( equal && c == value ) || ( !equal && c != value ) ) {
count += 1;
}
}
}
return count;
}
71 void addStaticLayer(
nav2_costmap_2d::LayeredCostmap & layers,
tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node,
std::shared_ptr<nav2_costmap_2d::StaticLayer> & slayer,
rclcpp::CallbackGroup::SharedPtr callback_group = nullptr )
{
slayer = std::make_shared<nav2_costmap_2d::StaticLayer>( );
layers.addPlugin( std::shared_ptr<nav2_costmap_2d::Layer>( slayer ) );
slayer->initialize( &layers, "static", &tf, node, callback_group );
}
82 void addObstacleLayer(
nav2_costmap_2d::LayeredCostmap & layers,
tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node,
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> & olayer,
rclcpp::CallbackGroup::SharedPtr callback_group = nullptr )
{
olayer = std::make_shared<nav2_costmap_2d::ObstacleLayer>( );
olayer->initialize( &layers, "obstacles", &tf, node, callback_group );
layers.addPlugin( std::shared_ptr<nav2_costmap_2d::Layer>( olayer ) );
}
93 void addRangeLayer(
nav2_costmap_2d::LayeredCostmap & layers,
tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node,
std::shared_ptr<nav2_costmap_2d::RangeSensorLayer> & rlayer,
rclcpp::CallbackGroup::SharedPtr callback_group = nullptr )
{
rlayer = std::make_shared<nav2_costmap_2d::RangeSensorLayer>( );
rlayer->initialize( &layers, "range", &tf, node, callback_group );
layers.addPlugin( std::shared_ptr<nav2_costmap_2d::Layer>( rlayer ) );
}
104 void addObservation(
std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer, double x, double y, double z = 0.0,
double ox = 0.0, double oy = 0.0, double oz = MAX_Z, bool marking = true, bool clearing = true,
double raytrace_max_range = 100.0,
double raytrace_min_range = 0.0,
double obstacle_max_range = 100.0,
double obstacle_min_range = 0.0 )
{
sensor_msgs::msg::PointCloud2 cloud;
sensor_msgs::PointCloud2Modifier modifier( cloud );
modifier.setPointCloud2FieldsByString( 1, "xyz" );
modifier.resize( 1 );
sensor_msgs::PointCloud2Iterator<float> iter_x( cloud, "x" );
sensor_msgs::PointCloud2Iterator<float> iter_y( cloud, "y" );
sensor_msgs::PointCloud2Iterator<float> iter_z( cloud, "z" );
*iter_x = x;
*iter_y = y;
*iter_z = z;
geometry_msgs::msg::Point p;
p.x = ox;
p.y = oy;
p.z = oz;
nav2_costmap_2d::Observation obs( p, cloud, obstacle_max_range, obstacle_min_range,
raytrace_max_range, raytrace_min_range );
olayer->addStaticObservation( obs, marking, clearing );
}
133 void addInflationLayer(
nav2_costmap_2d::LayeredCostmap & layers,
tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node,
std::shared_ptr<nav2_costmap_2d::InflationLayer> & ilayer,
rclcpp::CallbackGroup::SharedPtr callback_group = nullptr )
{
ilayer = std::make_shared<nav2_costmap_2d::InflationLayer>( );
ilayer->initialize( &layers, "inflation", &tf, node, callback_group );
std::shared_ptr<nav2_costmap_2d::Layer> ipointer( ilayer );
layers.addPlugin( ipointer );
}
#endif // TESTING_HELPER_HPP_
1 /*
* Copyright ( c ) 2012, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES ( INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE )
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <string>
#include <vector>
#include "gtest/gtest.h"
#include "nav2_costmap_2d/array_parser.hpp"
36 TEST( array_parser, basic_operation )
{
std::string error;
std::vector<std::vector<float>> vvf;
vvf = nav2_costmap_2d::parseVVF( "[[1, 2.2], [.3, -4e4]]", error );
EXPECT_EQ( 2u, vvf.size( ) );
EXPECT_EQ( 2u, vvf[0].size( ) );
EXPECT_EQ( 2u, vvf[1].size( ) );
EXPECT_EQ( 1.0f, vvf[0][0] );
EXPECT_EQ( 2.2f, vvf[0][1] );
EXPECT_EQ( 0.3f, vvf[1][0] );
EXPECT_EQ( -40000.0f, vvf[1][1] );
EXPECT_EQ( "", error );
}
51 TEST( array_parser, missing_open )
{
std::string error;
std::vector<std::vector<float>> vvf;
vvf = nav2_costmap_2d::parseVVF( "[1, 2.2], [.3, -4e4]]", error );
EXPECT_NE( error, "" );
}
59 TEST( array_parser, missing_close )
{
std::string error;
std::vector<std::vector<float>> vvf;
vvf = nav2_costmap_2d::parseVVF( "[[1, 2.2], [.3, -4e4]", error );
EXPECT_NE( error, "" );
}
67 TEST( array_parser, wrong_depth )
{
std::string error;
std::vector<std::vector<float>> vvf;
vvf = nav2_costmap_2d::parseVVF( "[1, 2.2], [.3, -4e4]", error );
EXPECT_NE( error, "" );
}
75 int main( int argc, char ** argv )
{
testing::InitGoogleTest( &argc, argv );
return RUN_ALL_TESTS( );
}
1 // Copyright ( c ) 2021 Samsung Research Russia
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <gtest/gtest.h>
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
20 class RclCppFixture
{
public:
23 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
24 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
28 TEST( CopyWindow, copyValidWindow )
{
nav2_costmap_2d::Costmap2D src( 10, 10, 0.1, 0.0, 0.0 );
nav2_costmap_2d::Costmap2D dst( 5, 5, 0.2, 100.0, 100.0 );
// Adding 2 marked cells to source costmap
src.setCost( 2, 2, 100 );
src.setCost( 5, 5, 200 );
ASSERT_TRUE( dst.copyWindow( src, 2, 2, 6, 6, 0, 0 ) );
// Check that both marked cells were copied to destination costmap
ASSERT_EQ( dst.getCost( 0, 0 ), 100 );
ASSERT_EQ( dst.getCost( 3, 3 ), 200 );
}
42 TEST( CopyWindow, copyInvalidWindow )
{
nav2_costmap_2d::Costmap2D src( 10, 10, 0.1, 0.0, 0.0 );
nav2_costmap_2d::Costmap2D dst( 5, 5, 0.2, 100.0, 100.0 );
// Case1: incorrect source bounds
ASSERT_FALSE( dst.copyWindow( src, 9, 9, 11, 11, 0, 0 ) );
// Case2: incorrect destination bounds
ASSERT_FALSE( dst.copyWindow( src, 0, 0, 1, 1, 5, 5 ) );
ASSERT_FALSE( dst.copyWindow( src, 0, 0, 6, 6, 0, 0 ) );
}
1 // Copyright ( c ) 2020 Samsung Research Russia
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <gtest/gtest.h>
#include <vector>
#include <cmath>
#include <limits>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "nav2_util/occ_grid_values.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "nav2_costmap_2d/cost_values.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
28 class RclCppFixture
{
public:
31 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
32 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
36 static constexpr double EPSILON = std::numeric_limits<float>::epsilon( );
37 static constexpr double RESOLUTION = 0.05;
38 static constexpr double ORIGIN_X = 0.1;
39 static constexpr double ORIGIN_Y = 0.2;
41 class TestNode : public ::testing::Test
{
public:
TestNode( ) {}
~TestNode( )
{
occ_grid_.reset( );
costmap_.reset( );
}
protected:
void createMaps( );
void verifyCostmap( );
private:
std::shared_ptr<nav_msgs::msg::OccupancyGrid> occ_grid_;
std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_;
};
61 void TestNode::createMaps( )
{
// Create occ_grid_ map
occ_grid_ = std::make_shared<nav_msgs::msg::OccupancyGrid>( );
const unsigned int width = 4;
const unsigned int height = 3;
occ_grid_->info.resolution = RESOLUTION;
occ_grid_->info.width = width;
occ_grid_->info.height = height;
occ_grid_->info.origin.position.x = ORIGIN_X;
occ_grid_->info.origin.position.y = ORIGIN_Y;
occ_grid_->info.origin.position.z = 0.0;
occ_grid_->info.origin.orientation.x = 0.0;
occ_grid_->info.origin.orientation.y = 0.0;
occ_grid_->info.origin.orientation.z = 0.0;
occ_grid_->info.origin.orientation.w = 1.0;
occ_grid_->data.resize( width * height );
int8_t data;
for ( unsigned int i = 0; i < width * height; i++ ) {
data = i * 10;
if ( data <= nav2_util::OCC_GRID_OCCUPIED ) {
occ_grid_->data[i] = data;
} else {
occ_grid_->data[i] = nav2_util::OCC_GRID_UNKNOWN;
}
}
// Create costmap_ ( convert OccupancyGrid -> to Costmap2D )
costmap_ = std::make_shared<nav2_costmap_2d::Costmap2D>( *occ_grid_ );
}
95 void TestNode::verifyCostmap( )
{
// Verify Costmap2D info
EXPECT_NEAR( costmap_->getResolution( ), RESOLUTION, EPSILON );
EXPECT_NEAR( costmap_->getOriginX( ), ORIGIN_X, EPSILON );
EXPECT_NEAR( costmap_->getOriginY( ), ORIGIN_Y, EPSILON );
// Verify Costmap2D data
unsigned int it;
unsigned char data, data_ref;
for ( it = 0; it < ( costmap_->getSizeInCellsX( ) * costmap_->getSizeInCellsY( ) - 1 ); it++ ) {
data = costmap_->getCharMap( )[it];
if ( it != costmap_->getSizeInCellsX( ) * costmap_->getSizeInCellsY( ) - 1 ) {
data_ref = std::round(
static_cast<double>( nav2_costmap_2d::LETHAL_OBSTACLE - nav2_costmap_2d::FREE_SPACE ) * it /
10 );
} else {
data_ref = nav2_costmap_2d::NO_INFORMATION;
}
EXPECT_EQ( data, data_ref );
}
}
118 TEST_F( TestNode, convertOccGridToCostmap )
{
createMaps( );
verifyCostmap( );
}
1 // Copyright ( c ) 2020 Samsung Research Russia
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <gtest/gtest.h>
#include <string>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/layer.hpp"
23 class RclCppFixture
{
public:
26 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
27 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
31 class LayerWrapper : public nav2_costmap_2d::Layer
{
33 void reset( ) {}
34 void updateBounds( double, double, double, double *, double *, double *, double * ) {}
35 void updateCosts( nav2_costmap_2d::Costmap2D &, int, int, int, int ) {}
36 bool isClearable( ) {return false;}
};
39 TEST( DeclareParameter, useValidParameter )
{
LayerWrapper layer;
nav2_util::LifecycleNode::SharedPtr node =
std::make_shared<nav2_util::LifecycleNode>( "test_node" );
tf2_ros::Buffer tf( node->get_clock( ) );
nav2_costmap_2d::LayeredCostmap layers( "frame", false, false );
layer.initialize( &layers, "test_layer", &tf, node, nullptr );
layer.declareParameter( "test1", rclcpp::ParameterValue( "test_val1" ) );
try {
std::string val = node->get_parameter( "test_layer.test1" ).as_string( );
EXPECT_EQ( val, "test_val1" );
} catch ( rclcpp::exceptions::ParameterNotDeclaredException & ex ) {
FAIL( ) << "test_layer.test1 parameter is not set";
}
}
58 TEST( DeclareParameter, useInvalidParameter )
{
LayerWrapper layer;
nav2_util::LifecycleNode::SharedPtr node =
std::make_shared<nav2_util::LifecycleNode>( "test_node" );
tf2_ros::Buffer tf( node->get_clock( ) );
nav2_costmap_2d::LayeredCostmap layers( "frame", false, false );
layer.initialize( &layers, "test_layer", &tf, node, nullptr );
layer.declareParameter( "test2", rclcpp::PARAMETER_STRING );
try {
std::string val = node->get_parameter( "test_layer.test2" ).as_string( );
FAIL( ) << "Incorrectly handling test_layer.test2 parameter which was not set";
} catch ( rclcpp::exceptions::ParameterUninitializedException & ex ) {
SUCCEED( );
}
}
1 // Copyright ( c ) 2020 Shivang Patel
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <vector>
#include <memory>
#include "gtest/gtest.h"
#include "nav2_costmap_2d/footprint_collision_checker.hpp"
#include "nav2_costmap_2d/footprint.hpp"
23 TEST( collision_footprint, test_basic )
{
std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
std::make_shared<nav2_costmap_2d::Costmap2D>( 100, 100, 0.1, 0, 0, 0 );
geometry_msgs::msg::Point p1;
p1.x = -0.5;
p1.y = 0.0;
geometry_msgs::msg::Point p2;
p2.x = 0.0;
p2.y = 0.5;
geometry_msgs::msg::Point p3;
p3.x = 0.5;
p3.y = 0.0;
geometry_msgs::msg::Point p4;
p4.x = 0.0;
p4.y = -0.5;
nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
collision_checker( costmap_ );
auto value = collision_checker.footprintCostAtPose( 5.0, 5.0, 0.0, footprint );
EXPECT_NEAR( value, 0.0, 0.001 );
}
51 TEST( collision_footprint, test_point_cost )
{
std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
std::make_shared<nav2_costmap_2d::Costmap2D>( 100, 100, 0.1, 0, 0, 0 );
nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
collision_checker( costmap_ );
auto value = collision_checker.pointCost( 50, 50 );
EXPECT_NEAR( value, 0.0, 0.001 );
}
64 TEST( collision_footprint, test_world_to_map )
{
std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
std::make_shared<nav2_costmap_2d::Costmap2D>( 100, 100, 0.1, 0, 0, 0 );
nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
collision_checker( costmap_ );
unsigned int x, y;
collision_checker.worldToMap( 1.0, 1.0, x, y );
auto value = collision_checker.pointCost( x, y );
EXPECT_NEAR( value, 0.0, 0.001 );
costmap_->setCost( 50, 50, 200 );
collision_checker.worldToMap( 5.0, 5.0, x, y );
EXPECT_NEAR( collision_checker.pointCost( x, y ), 200.0, 0.001 );
}
86 TEST( collision_footprint, test_footprint_at_pose_with_movement )
{
std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
std::make_shared<nav2_costmap_2d::Costmap2D>( 100, 100, 0.1, 0, 0, 254 );
for ( unsigned int i = 40; i <= 60; ++i ) {
for ( unsigned int j = 40; j <= 60; ++j ) {
costmap_->setCost( i, j, 0 );
}
}
geometry_msgs::msg::Point p1;
p1.x = -1.0;
p1.y = 1.0;
geometry_msgs::msg::Point p2;
p2.x = 1.0;
p2.y = 1.0;
geometry_msgs::msg::Point p3;
p3.x = 1.0;
p3.y = -1.0;
geometry_msgs::msg::Point p4;
p4.x = -1.0;
p4.y = -1.0;
nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
collision_checker( costmap_ );
auto value = collision_checker.footprintCostAtPose( 5.0, 5.0, 0.0, footprint );
EXPECT_NEAR( value, 0.0, 0.001 );
auto up_value = collision_checker.footprintCostAtPose( 5.0, 4.9, 0.0, footprint );
EXPECT_NEAR( up_value, 254.0, 0.001 );
auto down_value = collision_checker.footprintCostAtPose( 5.0, 5.2, 0.0, footprint );
EXPECT_NEAR( down_value, 254.0, 0.001 );
}
125 TEST( collision_footprint, test_point_and_line_cost )
{
std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ =
std::make_shared<nav2_costmap_2d::Costmap2D>( 100, 100, 0.10000, 0, 0.0, 0.0 );
costmap_->setCost( 62, 50, 254 );
costmap_->setCost( 39, 60, 254 );
geometry_msgs::msg::Point p1;
p1.x = -1.0;
p1.y = 1.0;
geometry_msgs::msg::Point p2;
p2.x = 1.0;
p2.y = 1.0;
geometry_msgs::msg::Point p3;
p3.x = 1.0;
p3.y = -1.0;
geometry_msgs::msg::Point p4;
p4.x = -1.0;
p4.y = -1.0;
nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
nav2_costmap_2d::FootprintCollisionChecker<std::shared_ptr<nav2_costmap_2d::Costmap2D>>
collision_checker( costmap_ );
auto value = collision_checker.footprintCostAtPose( 5.0, 5.0, 0.0, footprint );
EXPECT_NEAR( value, 0.0, 0.001 );
auto left_value = collision_checker.footprintCostAtPose( 4.9, 5.0, 0.0, footprint );
EXPECT_NEAR( left_value, 254.0, 0.001 );
auto right_value = collision_checker.footprintCostAtPose( 5.2, 5.0, 0.0, footprint );
EXPECT_NEAR( right_value, 254.0, 0.001 );
}
161 TEST( collision_footprint, not_enough_points )
{
geometry_msgs::msg::Point p1;
p1.x = 2.0;
p1.y = 2.0;
geometry_msgs::msg::Point p2;
p2.x = -2.0;
p2.y = -2.0;
std::vector<geometry_msgs::msg::Point> footprint = {p1, p2};
double min_dist = 0.0;
double max_dist = 0.0;
nav2_costmap_2d::calculateMinAndMaxDistances( footprint, min_dist, max_dist );
EXPECT_EQ( min_dist, std::numeric_limits<double>::max( ) );
EXPECT_EQ( max_dist, 0.0f );
}
180 TEST( collision_footprint, to_point_32 ) {
geometry_msgs::msg::Point p;
p.x = 123.0;
p.y = 456.0;
p.z = 789.0;
geometry_msgs::msg::Point32 p32;
p32 = nav2_costmap_2d::toPoint32( p );
EXPECT_NEAR( p.x, p32.x, 1e-5 );
EXPECT_NEAR( p.y, p32.y, 1e-5 );
EXPECT_NEAR( p.z, p32.z, 1e-5 );
}
193 TEST( collision_footprint, to_polygon ) {
geometry_msgs::msg::Point p1;
p1.x = 1.2;
p1.y = 3.4;
p1.z = 5.1;
geometry_msgs::msg::Point p2;
p2.x = -5.6;
p2.y = -7.8;
p2.z = -9.1;
std::vector<geometry_msgs::msg::Point> pts = {p1, p2};
geometry_msgs::msg::Polygon poly;
poly = nav2_costmap_2d::toPolygon( pts );
EXPECT_EQ( 2u, sizeof( poly.points ) / sizeof( poly.points[0] ) );
EXPECT_NEAR( poly.points[0].x, p1.x, 1e-5 );
EXPECT_NEAR( poly.points[0].y, p1.y, 1e-5 );
EXPECT_NEAR( poly.points[0].z, p1.z, 1e-5 );
EXPECT_NEAR( poly.points[1].x, p2.x, 1e-5 );
EXPECT_NEAR( poly.points[1].y, p2.y, 1e-5 );
EXPECT_NEAR( poly.points[1].z, p2.z, 1e-5 );
}
217 TEST( collision_footprint, make_footprint_from_string ) {
std::vector<geometry_msgs::msg::Point> footprint;
bool result = nav2_costmap_2d::makeFootprintFromString(
"[[1, 2.2], [.3, -4e4], [-.3, -4e4], [-1, 2.2]]", footprint );
EXPECT_EQ( result, true );
EXPECT_EQ( 4u, footprint.size( ) );
EXPECT_NEAR( footprint[0].x, 1.0, 1e-5 );
EXPECT_NEAR( footprint[0].y, 2.2, 1e-5 );
EXPECT_NEAR( footprint[1].x, 0.3, 1e-5 );
EXPECT_NEAR( footprint[1].y, -4e4, 1e-5 );
EXPECT_NEAR( footprint[2].x, -0.3, 1e-5 );
EXPECT_NEAR( footprint[2].y, -4e4, 1e-5 );
EXPECT_NEAR( footprint[3].x, -1.0, 1e-5 );
EXPECT_NEAR( footprint[3].y, 2.2, 1e-5 );
}
233 TEST( collision_footprint, make_footprint_from_string_parse_error ) {
std::vector<geometry_msgs::msg::Point> footprint;
bool result = nav2_costmap_2d::makeFootprintFromString(
"[[bad_string", footprint );
EXPECT_EQ( result, false );
}
240 TEST( collision_footprint, make_footprint_from_string_two_points_error ) {
std::vector<geometry_msgs::msg::Point> footprint;
bool result = nav2_costmap_2d::makeFootprintFromString(
"[[1, 2.2], [.3, -4e4]", footprint );
EXPECT_EQ( result, false );
}
247 TEST( collision_footprint, make_footprint_from_string_not_pairs ) {
std::vector<geometry_msgs::msg::Point> footprint;
bool result = nav2_costmap_2d::makeFootprintFromString(
"[[1, 2.2], [.3, -4e4], [-.3, -4e4], [-1, 2.2, 5.6]]", footprint );
EXPECT_EQ( result, false );
}
/*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef COSTMAP_QUEUE__COSTMAP_QUEUE_HPP_
#define COSTMAP_QUEUE__COSTMAP_QUEUE_HPP_
#include <cmath>
#include <vector>
#include <limits>
#include <memory>
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "costmap_queue/map_based_queue.hpp"
namespace costmap_queue
{
/**
* @class CellData
* @brief Storage for cell information used during queue expansion
*/
51 class CellData
{
public:
/**
* @brief Real Constructor
* @param d The distance to the nearest obstacle
* @param i The index of the cell in the costmap. Redundant with the following two parameters.
* @param x The x coordinate of the cell in the cost map
* @param y The y coordinate of the cell in the cost map
* @param sx The x coordinate of the closest source cell in the costmap
* @param sy The y coordinate of the closest source cell in the costmap
*/
63 CellData(
const double d, const unsigned int i, const unsigned int x, const unsigned int y,
const unsigned int sx, const unsigned int sy )
: distance_( d ), index_( i ), x_( x ), y_( y ), src_x_( sx ), src_y_( sy )
{
}
/**
* @brief Default Constructor - Should be used sparingly
*/
CellData( )
: distance_( std::numeric_limits<double>::max( ) ), index_( 0 ), x_( 0 ), y_( 0 ), src_x_( 0 ), src_y_( 0 )
{
}
static unsigned absolute_difference( const unsigned x, const unsigned y )
{
return ( x > y ) ? ( x - y ) : ( y - x );
}
double distance_;
unsigned int index_;
unsigned int x_, y_;
unsigned int src_x_, src_y_;
};
/**
* @class CostmaQueue
* @brief A tool for finding the cells closest to some set of originating cells.
*
* A common operation with costmaps is to define a set of cells in the costmap, and then
* perform some operation on all the other cells based on which cell in the original set
* the other cells are closest to. This operation is done in the inflation layer to figure out
* how far each cell is from an obstacle, and is also used in a number of Trajectory cost functions.
*
* It is implemented with a queue. The standard operation is to enqueueCell the original set, and then
* retreive the other cells with the isEmpty/getNextCell iterator-like functionality. getNextCell
* returns an object that contains the coordinates of this cell and the origin cell, as well as
* the distance between them. By default, the Euclidean distance is used for ordering, but passing in
* manhattan=true to the constructor will use the Manhattan distance.
*
* The validCellToQueue overridable-function allows for deriving classes to limit the queue traversal
* to a subset of all costmap cells. LimitedCostmapQueue does this by ignoring distances above a limit.
*
*/
class CostmapQueue : public MapBasedQueue<CellData>
{
public:
/**
* @brief constructor
* @param costmap Costmap which defines the size/number of cells
* @param manhattan If true, sort cells by Manhattan distance, otherwise use Euclidean distance
*/
explicit CostmapQueue( nav2_costmap_2d::Costmap2D & costmap, bool manhattan = false );
/**
* @brief Clear the queue
*/
void reset( ) override;
/**
* @brief Add a cell the queue
* @param x X coordinate of the cell
* @param y Y coordinate of the cell
*/
void enqueueCell( unsigned int x, unsigned int y );
/**
* @brief Get the next cell to examine, and enqueue its neighbors as needed
* @return The next cell
*
* NB: Assumes that isEmpty has been called before this call and returned false
*/
CellData getNextCell( );
/**
* @brief Check to see if we should add this cell to the queue. Always true unless overridden.
* @param cell The cell to check
* @return True, unless overriden
*/
virtual bool validCellToQueue( const CellData & /*cell*/ ) {return true;}
/**
* @brief convenience typedef for a pointer
*/
typedef std::shared_ptr<CostmapQueue> Ptr;
protected:
/**
* @brief Enqueue a cell with the given coordinates and the given source cell
*/
void enqueueCell(
unsigned int index, unsigned int cur_x, unsigned int cur_y, unsigned int src_x,
unsigned int src_y );
/**
* @brief Compute the cached distances
*/
void computeCache( );
nav2_costmap_2d::Costmap2D & costmap_;
std::vector<bool> seen_;
int max_distance_;
bool manhattan_;
protected:
/**
* @brief Lookup pre-computed distances
* @param cur_x The x coordinate of the current cell
* @param cur_y The y coordinate of the current cell
* @param src_x The x coordinate of the source cell
* @param src_y The y coordinate of the source cell
* @return
*/
inline double distanceLookup(
const unsigned int cur_x, const unsigned int cur_y,
const unsigned int src_x, const unsigned int src_y )
{
unsigned int dx = CellData::absolute_difference( cur_x, src_x );
unsigned int dy = CellData::absolute_difference( cur_y, src_y );
return cached_distances_[dx][dy];
}
std::vector<std::vector<double>> cached_distances_;
int cached_max_distance_;
};
} // namespace costmap_queue
#endif // COSTMAP_QUEUE__COSTMAP_QUEUE_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef COSTMAP_QUEUE__LIMITED_COSTMAP_QUEUE_HPP_
#define COSTMAP_QUEUE__LIMITED_COSTMAP_QUEUE_HPP_
#include "costmap_queue/costmap_queue.hpp"
namespace costmap_queue
{
/**
* @class LimitedCostmapQueue
* @brief Extension of Costmap Queue where distances are limited to a given distance from source cells.
*/
47 class LimitedCostmapQueue : public CostmapQueue
{
public:
/**
* @brief Constructor with limit as an integer number of cells.
*/
53 LimitedCostmapQueue( nav2_costmap_2d::Costmap2D & costmap, const int cell_distance_limit );
bool validCellToQueue( const CellData & cell ) override;
};
} // namespace costmap_queue
#endif // COSTMAP_QUEUE__LIMITED_COSTMAP_QUEUE_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef COSTMAP_QUEUE__MAP_BASED_QUEUE_HPP_
#define COSTMAP_QUEUE__MAP_BASED_QUEUE_HPP_
#include <algorithm>
#include <map>
#include <stdexcept>
#include <utility>
#include <vector>
namespace costmap_queue
{
/**
* @brief Templatized interface for a priority queue
*
* This is faster than the std::priority_queue implementation in certain cases because iterating does
* not require resorting after every element is examined.
* Based on https://github.com/ros-planning/navigation/pull/525
* The relative speed of this against the priority queue depends how many items with each
* priority are inserted into the queue.
*
* One additional speed up depends on the patterns of priorities during each iteration of the queue.
* If the same priorities are inserted into the queue on every iteration, then it is quicker to
* set reset_bins = false, such that the priority bins are not reset and will not have to be recreated
* on each iteration.
*/
template<class item_t>
61 class MapBasedQueue
{
public:
/**
* @brief Default Constructor
*/
67 explicit MapBasedQueue( bool reset_bins = true )
: reset_bins_( reset_bins ), item_count_( 0 )
{
reset( );
}
/**
* @brief Clear the queue
*/
76 virtual void reset( )
{
if ( reset_bins_ || item_count_ > 0 ) {
item_bins_.clear( );
item_count_ = 0;
}
iter_ = last_insert_iter_ = item_bins_.end( );
}
/**
* @brief Add a new item to the queue with a set priority
* @param priority Priority of the item
* @param item Payload item
*/
90 void enqueue( const double priority, item_t item )
{
// We keep track of the last priority we inserted. If this items priority
// matches the previous insertion we can avoid searching through all the
// bins.
if ( last_insert_iter_ == item_bins_.end( ) || last_insert_iter_->first != priority ) {
last_insert_iter_ = item_bins_.find( priority );
// If not found, create a new bin
if ( last_insert_iter_ == item_bins_.end( ) ) {
auto map_item = std::make_pair( priority, std::move( std::vector<item_t>( ) ) );
// Inserts an item if it doesn't exist. Returns an iterator to the item
// whether it existed or was inserted.
std::pair<ItemMapIterator, bool> insert_result = item_bins_.insert( std::move( map_item ) );
last_insert_iter_ = insert_result.first;
}
}
// Add the item to the vector for this map key
last_insert_iter_->second.push_back( item );
item_count_++;
// Use short circuiting to check if we want to update the iterator
if ( iter_ == item_bins_.end( ) || priority < iter_->first ) {
iter_ = last_insert_iter_;
}
}
/**
* @brief Check to see if there is anything in the queue
* @return True if there is nothing in the queue
*
* Must be called prior to front/pop.
*/
125 bool isEmpty( )
{
return item_count_ == 0;
}
/**
* @brief Return the item at the front of the queue
* @return The item at the front of the queue
*/
134 item_t & front( )
{
if ( iter_ == item_bins_.end( ) ) {
throw std::out_of_range( "front( ) called on empty costmap_queue::MapBasedQueue!" );
}
return iter_->second.back( );
}
/**
* @brief Remove ( and destroy ) the item at the front of the queue
*/
146 void pop( )
{
if ( iter_ != item_bins_.end( ) && !iter_->second.empty( ) ) {
iter_->second.pop_back( );
item_count_--;
}
auto not_empty = []( const typename ItemMap::value_type & key_val ) {
return !key_val.second.empty( );
};
iter_ = std::find_if( iter_, item_bins_.end( ), not_empty );
}
protected:
using ItemMap = std::map<double, std::vector<item_t>>;
using ItemMapIterator = typename ItemMap::iterator;
bool reset_bins_;
ItemMap item_bins_;
unsigned int item_count_;
ItemMapIterator iter_;
ItemMapIterator last_insert_iter_;
};
} // namespace costmap_queue
#endif // COSTMAP_QUEUE__MAP_BASED_QUEUE_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "costmap_queue/costmap_queue.hpp"
#include <algorithm>
#include <cmath>
#include <vector>
using std::hypot;
namespace costmap_queue
{
44 CostmapQueue::CostmapQueue( nav2_costmap_2d::Costmap2D & costmap, bool manhattan )
: MapBasedQueue( ), costmap_( costmap ), max_distance_( -1 ), manhattan_( manhattan ),
cached_max_distance_( -1 )
{
reset( );
}
51 void CostmapQueue::reset( )
{
unsigned int size_x = costmap_.getSizeInCellsX( ), size_y = costmap_.getSizeInCellsY( );
if ( seen_.size( ) != size_x * size_y ) {
seen_.resize( size_x * size_y );
}
std::fill( seen_.begin( ), seen_.end( ), false );
computeCache( );
MapBasedQueue::reset( );
}
62 void CostmapQueue::enqueueCell( unsigned int x, unsigned int y )
{
unsigned int index = costmap_.getIndex( x, y );
enqueueCell( index, x, y, x, y );
}
68 void CostmapQueue::enqueueCell(
unsigned int index, unsigned int cur_x, unsigned int cur_y,
unsigned int src_x, unsigned int src_y )
{
if ( seen_[index] ) {return;}
// we compute our distance table one cell further than the inflation radius
// dictates so we can make the check below
double distance = distanceLookup( cur_x, cur_y, src_x, src_y );
CellData data( distance, index, cur_x, cur_y, src_x, src_y );
if ( validCellToQueue( data ) ) {
seen_[index] = true;
enqueue( distance, data );
}
}
84 CellData CostmapQueue::getNextCell( )
{
// get the highest priority cell and pop it off the priority queue
CellData current_cell = front( );
pop( );
unsigned int index = current_cell.index_;
unsigned int mx = current_cell.x_;
unsigned int my = current_cell.y_;
unsigned int sx = current_cell.src_x_;
unsigned int sy = current_cell.src_y_;
// attempt to put the neighbors of the current cell onto the queue
unsigned int size_x = costmap_.getSizeInCellsX( );
if ( mx > 0 ) {
enqueueCell( index - 1, mx - 1, my, sx, sy );
}
if ( my > 0 ) {
enqueueCell( index - size_x, mx, my - 1, sx, sy );
}
if ( mx < size_x - 1 ) {
enqueueCell( index + 1, mx + 1, my, sx, sy );
}
if ( my < costmap_.getSizeInCellsY( ) - 1 ) {
enqueueCell( index + size_x, mx, my + 1, sx, sy );
}
return current_cell;
}
114 void CostmapQueue::computeCache( )
{
if ( max_distance_ == -1 ) {
max_distance_ = std::max( costmap_.getSizeInCellsX( ), costmap_.getSizeInCellsY( ) );
}
if ( max_distance_ == cached_max_distance_ ) {return;}
cached_distances_.clear( );
cached_distances_.resize( max_distance_ + 2 );
for ( unsigned int i = 0; i < cached_distances_.size( ); ++i ) {
cached_distances_[i].resize( max_distance_ + 2 );
for ( unsigned int j = 0; j < cached_distances_[i].size( ); ++j ) {
if ( manhattan_ ) {
cached_distances_[i][j] = i + j;
} else {
cached_distances_[i][j] = hypot( i, j );
}
}
}
cached_max_distance_ = max_distance_;
}
} // namespace costmap_queue
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "costmap_queue/limited_costmap_queue.hpp"
namespace costmap_queue
{
40 LimitedCostmapQueue::LimitedCostmapQueue(
41 nav2_costmap_2d::Costmap2D & costmap,
const int distance_limit )
: CostmapQueue( costmap )
{
max_distance_ = distance_limit;
reset( );
}
49 bool LimitedCostmapQueue::validCellToQueue( const CellData & cell )
{
return cell.distance_ <= max_distance_;
}
} // namespace costmap_queue
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <string>
#include "gtest/gtest.h"
#include "costmap_queue/map_based_queue.hpp"
using costmap_queue::MapBasedQueue;
41 void letter_test( MapBasedQueue<char> & q, const char test_letter )
{
ASSERT_FALSE( q.isEmpty( ) );
char c = q.front( );
EXPECT_EQ( c, test_letter );
q.pop( );
}
49 TEST( MapBasedQueue, emptyQueue )
{
MapBasedQueue<char> q;
EXPECT_TRUE( q.isEmpty( ) );
q.enqueue( 1.0, 'A' );
EXPECT_FALSE( q.isEmpty( ) );
}
57 TEST( MapBasedQueue, checkOrdering )
{
MapBasedQueue<char> q;
q.enqueue( 1.0, 'A' );
q.enqueue( 3.0, 'B' );
q.enqueue( 2.0, 'C' );
q.enqueue( 5.0, 'D' );
q.enqueue( 0.0, 'E' );
std::string expected = "EACBD";
for ( unsigned int i = 0; i < expected.size( ); i++ ) {
letter_test( q, expected[i] );
}
EXPECT_TRUE( q.isEmpty( ) );
}
73 TEST( MapBasedQueue, checkDynamicOrdering )
{
MapBasedQueue<char> q;
q.enqueue( 1.0, 'A' );
q.enqueue( 3.0, 'B' );
q.enqueue( 2.0, 'C' );
q.enqueue( 5.0, 'D' );
std::string expected = "ACB";
for ( unsigned int i = 0; i < expected.size( ); i++ ) {
letter_test( q, expected[i] );
}
q.enqueue( 0.0, 'E' );
letter_test( q, 'E' );
}
90 TEST( MapBasedQueue, checkDynamicOrdering2 )
{
MapBasedQueue<char> q;
q.enqueue( 1.0, 'A' );
q.enqueue( 2.0, 'B' );
letter_test( q, 'A' );
q.enqueue( 3.0, 'C' );
letter_test( q, 'B' );
}
100 TEST( MapBasedQueue, checkDynamicOrdering3 )
{
MapBasedQueue<char> q;
q.enqueue( 1.0, 'A' );
q.enqueue( 2.0, 'B' );
q.enqueue( 5.0, 'D' );
letter_test( q, 'A' );
letter_test( q, 'B' );
q.enqueue( 1.0, 'C' );
letter_test( q, 'C' );
letter_test( q, 'D' );
}
113 int main( int argc, char ** argv )
{
testing::InitGoogleTest( &argc, argv );
return RUN_ALL_TESTS( );
}
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <cmath>
#include <memory>
#include <algorithm>
#include "gtest/gtest.h"
#include "costmap_queue/costmap_queue.hpp"
#include "costmap_queue/limited_costmap_queue.hpp"
#include "rclcpp/rclcpp.hpp"
using std::hypot;
45 nav2_costmap_2d::Costmap2D costmap( 5, 5, 1.0, 0.0, 0.0 );
47 TEST( CostmapQueue, basicQueue )
{
costmap_queue::CostmapQueue q( costmap );
int count = 0;
q.enqueueCell( 0, 0 );
while ( !q.isEmpty( ) ) {
costmap_queue::CellData cell = q.getNextCell( );
EXPECT_EQ( cell.distance_, hypot( cell.x_, cell.y_ ) );
count++;
}
EXPECT_EQ( count, 25 );
}
60 TEST( CostmapQueue, bigTest )
{
nav2_costmap_2d::Costmap2D big_map( 500, 500, 1.0, 0.0, 0.0 );
costmap_queue::CostmapQueue q( big_map );
int count = 0;
q.enqueueCell( 0, 0 );
while ( !q.isEmpty( ) ) {
costmap_queue::CellData cell = q.getNextCell( );
EXPECT_EQ( cell.distance_, hypot( cell.x_, cell.y_ ) );
count++;
}
EXPECT_EQ( count, 500 * 500 );
}
74 TEST( CostmapQueue, linearQueue )
{
costmap_queue::CostmapQueue q( costmap );
int count = 0;
q.enqueueCell( 0, 0 );
q.enqueueCell( 0, 1 );
q.enqueueCell( 0, 2 );
q.enqueueCell( 0, 3 );
q.enqueueCell( 0, 4 );
while ( !q.isEmpty( ) ) {
costmap_queue::CellData cell = q.getNextCell( );
EXPECT_EQ( cell.distance_, cell.x_ );
count++;
}
EXPECT_EQ( count, 25 );
}
91 TEST( CostmapQueue, crossQueue )
{
costmap_queue::CostmapQueue q( costmap );
int count = 0;
int xs[] = {1, 2, 2, 3};
int ys[] = {2, 1, 3, 2};
int N = 4;
for ( int i = 0; i < N; i++ ) {
q.enqueueCell( xs[i], ys[i] );
}
while ( !q.isEmpty( ) ) {
costmap_queue::CellData cell = q.getNextCell( );
double min_d = 1000;
for ( int i = 0; i < N; i++ ) {
double dd = hypot( xs[i] - static_cast<float>( cell.x_ ), ys[i] - static_cast<float>( cell.y_ ) );
min_d = std::min( min_d, dd );
}
EXPECT_NEAR( cell.distance_, min_d, 0.00001 );
count++;
}
EXPECT_EQ( count, 25 );
}
116 TEST( CostmapQueue, limitedQueue )
{
costmap_queue::LimitedCostmapQueue q( costmap, 5 );
int count = 0;
q.enqueueCell( 0, 0 );
while ( !q.isEmpty( ) ) {
costmap_queue::CellData cell = q.getNextCell( );
EXPECT_EQ( cell.distance_, hypot( cell.x_, cell.y_ ) );
count++;
}
EXPECT_EQ( count, 24 );
costmap_queue::LimitedCostmapQueue q2( costmap, 3 );
count = 0;
q2.enqueueCell( 0, 0 );
while ( !q2.isEmpty( ) ) {
q2.getNextCell( );
count++;
}
EXPECT_EQ( count, 11 );
}
138 int main( int argc, char ** argv )
{
testing::InitGoogleTest( &argc, argv );
return RUN_ALL_TESTS( );
}
/*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CORE__DWB_LOCAL_PLANNER_HPP_
#define DWB_CORE__DWB_LOCAL_PLANNER_HPP_
#include <memory>
#include <string>
#include <vector>
#include "nav2_core/controller.hpp"
#include "nav2_core/goal_checker.hpp"
#include "dwb_core/publisher.hpp"
#include "dwb_core/trajectory_critic.hpp"
#include "dwb_core/trajectory_generator.hpp"
#include "nav_2d_msgs/msg/pose2_d_stamped.hpp"
#include "nav_2d_msgs/msg/twist2_d_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "pluginlib/class_loader.hpp"
#include "pluginlib/class_list_macros.hpp"
namespace dwb_core
{
/**
* @class DWBLocalPlanner
* @brief Plugin-based flexible controller
*/
61 class DWBLocalPlanner : public nav2_core::Controller
{
public:
/**
* @brief Constructor that brings up pluginlib loaders
*/
67 DWBLocalPlanner( );
void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros ) override;
virtual ~DWBLocalPlanner( ) {}
/**
* @brief Activate lifecycle node
*/
void activate( ) override;
/**
* @brief Deactivate lifecycle node
*/
84 void deactivate( ) override;
/**
* @brief Cleanup lifecycle node
*/
89 void cleanup( ) override;
/**
* @brief nav2_core setPlan - Sets the global plan
* @param path The global plan
*/
95 void setPlan( const nav_msgs::msg::Path & path ) override;
/**
* @brief nav2_core computeVelocityCommands - calculates the best command given the current pose and velocity
*
* It is presumed that the global plan is already set.
*
* This is mostly a wrapper for the protected computeVelocityCommands
* function which has additional debugging info.
*
* @param pose Current robot pose
* @param velocity Current robot velocity
* @param goal_checker Ptr to the goal checker for this task in case useful in computing commands
* @return The best command for the robot to drive
*/
110 geometry_msgs::msg::TwistStamped computeVelocityCommands(
const geometry_msgs::msg::PoseStamped & pose,
const geometry_msgs::msg::Twist & velocity,
nav2_core::GoalChecker * /*goal_checker*/ ) override;
/**
* @brief Score a given command. Can be used for testing.
*
* Given a trajectory, calculate the score where lower scores are better.
* If the given ( positive ) score exceeds the best_score, calculation may be cut short, as the
* score can only go up from there.
*
* @param traj Trajectory to check
* @param best_score If positive, the threshold for early termination
* @return The full scoring of the input trajectory
*/
126 virtual dwb_msgs::msg::TrajectoryScore scoreTrajectory(
const dwb_msgs::msg::Trajectory2D & traj,
double best_score = -1 );
/**
* @brief Compute the best command given the current pose and velocity, with possible debug information
*
* Same as above computeVelocityCommands, but with debug results.
* If the results pointer is not null, additional information about the twists
* evaluated will be in results after the call.
*
* @param pose Current robot pose
* @param velocity Current robot velocity
* @param results Output param, if not NULL, will be filled in with full evaluation results
* @return Best command
*/
virtual nav_2d_msgs::msg::Twist2DStamped computeVelocityCommands(
const nav_2d_msgs::msg::Pose2DStamped & pose,
const nav_2d_msgs::msg::Twist2D & velocity,
std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> & results );
/**
* @brief Limits the maximum linear speed of the robot.
* @param speed_limit expressed in absolute value ( in m/s )
* or in percentage from maximum robot speed.
* @param percentage Setting speed limit in percentage if true
* or in absolute values in false case.
*/
void setSpeedLimit( const double & speed_limit, const bool & percentage ) override
{
if ( traj_generator_ ) {
traj_generator_->setSpeedLimit( speed_limit, percentage );
}
}
protected:
/**
* @brief Helper method for two common operations for the operating on the global_plan
*
* Transforms the global plan ( stored in global_plan_ ) relative to the pose and saves it in
* transformed_plan and possibly publishes it. Then it takes the last pose and transforms it
* to match the local costmap's frame
*/
void prepareGlobalPlan(
const nav_2d_msgs::msg::Pose2DStamped & pose, nav_2d_msgs::msg::Path2D & transformed_plan,
nav_2d_msgs::msg::Pose2DStamped & goal_pose, bool publish_plan = true );
/**
* @brief Iterate through all the twists and find the best one
*/
virtual dwb_msgs::msg::TrajectoryScore coreScoringAlgorithm(
const geometry_msgs::msg::Pose2D & pose,
const nav_2d_msgs::msg::Twist2D velocity,
std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> & results );
/**
* @brief Transforms global plan into same frame as pose, clips far away poses and possibly prunes passed poses
*
* Three key operations
* 1 ) Transforms global plan into frame of the given pose
* 2 ) Only returns poses that are near the robot, i.e. whether they are likely on the local costmap
* 3 ) If prune_plan_ is true, it will remove all points that we've already passed from both the transformed plan
* and the saved global_plan_. Technically, it iterates to a pose on the path that is within prune_distance_
* of the robot and erases all poses before that.
*
* Additionally, shorten_transformed_plan_ determines whether we will pass the full plan all
* the way to the nav goal on to the critics or just a subset of the plan near the robot.
* True means pass just a subset. This gives DWB less discretion to decide how it gets to the
* nav goal. Instead it is encouraged to try to get on to the path generated by the global planner.
*/
virtual nav_2d_msgs::msg::Path2D transformGlobalPlan(
const nav_2d_msgs::msg::Pose2DStamped & pose );
nav_2d_msgs::msg::Path2D global_plan_; ///< Saved Global Plan
bool prune_plan_;
double prune_distance_;
bool debug_trajectory_details_;
rclcpp::Duration transform_tolerance_{0, 0};
bool shorten_transformed_plan_;
/**
* @brief try to resolve a possibly shortened critic name with the default namespaces and the suffix "Critic"
*
* @param base_name The name of the critic as read in from the parameter server
* @return Our attempted resolution of the name, with namespace prepended and/or the suffix Critic appended
*/
std::string resolveCriticClassName( std::string base_name );
/**
* @brief Load the critic parameters from the namespace
* @param name The namespace of this planner.
*/
virtual void loadCritics( );
rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
rclcpp::Clock::SharedPtr clock_;
rclcpp::Logger logger_{rclcpp::get_logger( "DWBLocalPlanner" )};
std::shared_ptr<tf2_ros::Buffer> tf_;
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
std::unique_ptr<DWBPublisher> pub_;
std::vector<std::string> default_critic_namespaces_;
// Plugin handling
pluginlib::ClassLoader<TrajectoryGenerator> traj_gen_loader_;
TrajectoryGenerator::Ptr traj_generator_;
pluginlib::ClassLoader<TrajectoryCritic> critic_loader_;
std::vector<TrajectoryCritic::Ptr> critics_;
std::string dwb_plugin_name_;
bool short_circuit_trajectory_evaluation_;
};
} // namespace dwb_core
#endif // DWB_CORE__DWB_LOCAL_PLANNER_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CORE__EXCEPTIONS_HPP_
#define DWB_CORE__EXCEPTIONS_HPP_
#include <stdexcept>
#include <string>
#include <memory>
#include "nav2_core/exceptions.hpp"
namespace dwb_core
{
/**
* @class PlannerTFException
* @brief Thrown when the planner cannot complete its operation due to TF errors
*/
50 class PlannerTFException : public nav2_core::PlannerException
{
public:
53 explicit PlannerTFException( const std::string description )
: nav2_core::PlannerException( description ) {}
};
/**
* @class IllegalTrajectoryException
* @brief Thrown when one of the critics encountered a fatal error
*/
61 class IllegalTrajectoryException : public nav2_core::PlannerException
{
public:
64 IllegalTrajectoryException( const std::string critic_name, const std::string description )
: nav2_core::PlannerException( description ), critic_name_( critic_name ) {}
66 std::string getCriticName( ) const {return critic_name_;}
protected:
69 std::string critic_name_;
};
} // namespace dwb_core
#endif // DWB_CORE__EXCEPTIONS_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CORE__ILLEGAL_TRAJECTORY_TRACKER_HPP_
#define DWB_CORE__ILLEGAL_TRAJECTORY_TRACKER_HPP_
#include <map>
#include <utility>
#include <string>
#include "dwb_core/exceptions.hpp"
#include "nav2_core/exceptions.hpp"
namespace dwb_core
{
46 class IllegalTrajectoryTracker
{
public:
49 IllegalTrajectoryTracker( )
: legal_count_( 0 ), illegal_count_( 0 ) {}
52 void addIllegalTrajectory( const IllegalTrajectoryException & e );
53 void addLegalTrajectory( );
55 std::map<std::pair<std::string, std::string>, double> getPercentages( ) const;
57 std::string getMessage( ) const;
protected:
60 std::map<std::pair<std::string, std::string>, unsigned int> counts_;
unsigned int legal_count_, illegal_count_;
};
/**
* @class NoLegalTrajectoriesException
* @brief Thrown when all the trajectories explored are illegal
*/
68 class NoLegalTrajectoriesException
69 : public nav2_core::PlannerException
{
public:
72 explicit NoLegalTrajectoriesException( const IllegalTrajectoryTracker & tracker )
: PlannerException( tracker.getMessage( ) ),
tracker_( tracker ) {}
75 IllegalTrajectoryTracker tracker_;
};
} // namespace dwb_core
#endif // DWB_CORE__ILLEGAL_TRAJECTORY_TRACKER_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CORE__PUBLISHER_HPP_
#define DWB_CORE__PUBLISHER_HPP_
#include <memory>
#include <string>
#include <vector>
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "dwb_core/trajectory_critic.hpp"
#include "dwb_msgs/msg/local_plan_evaluation.hpp"
#include "nav_msgs/msg/path.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "visualization_msgs/msg/marker_array.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "builtin_interfaces/msg/duration.hpp"
using rclcpp_lifecycle::LifecyclePublisher;
namespace dwb_core
{
/**
* @class DWBPublisher
* @brief Consolidation of all the publishing logic for the DWB Local Planner.
*
* Right now, it can publish
* 1 ) The Global Plan ( as passed in using setPath )
* 2 ) The Local Plan ( after it is calculated )
* 3 ) The Transformed Global Plan ( since it may be different than the global )
* 4 ) The Full LocalPlanEvaluation
* 5 ) Markers representing the different trajectories evaluated
* 6 ) The CostGrid ( in the form of a complex PointCloud2 )
*/
69 class DWBPublisher
{
public:
72 explicit DWBPublisher(
73 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
74 const std::string & plugin_name );
76 nav2_util::CallbackReturn on_configure( );
77 nav2_util::CallbackReturn on_activate( );
78 nav2_util::CallbackReturn on_deactivate( );
79 nav2_util::CallbackReturn on_cleanup( );
/**
* @brief Does the publisher require that the LocalPlanEvaluation be saved
* @return True if the Evaluation is needed to publish either directly or as trajectories
*/
85 bool shouldRecordEvaluation( ) {return publish_evaluation_ || publish_trajectories_;}
/**
* @brief If the pointer is not null, publish the evaluation and trajectories as needed
*/
90 void publishEvaluation( std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> results );
91 void publishLocalPlan(
92 const std_msgs::msg::Header & header,
93 const dwb_msgs::msg::Trajectory2D & traj );
94 void publishCostGrid(
95 const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,
96 const std::vector<TrajectoryCritic::Ptr> critics );
97 void publishGlobalPlan( const nav_2d_msgs::msg::Path2D plan );
98 void publishTransformedPlan( const nav_2d_msgs::msg::Path2D plan );
99 void publishLocalPlan( const nav_2d_msgs::msg::Path2D plan );
protected:
102 void publishTrajectories( const dwb_msgs::msg::LocalPlanEvaluation & results );
// Helper function for publishing other plans
105 void publishGenericPlan(
106 const nav_2d_msgs::msg::Path2D plan,
107 rclcpp::Publisher<nav_msgs::msg::Path> & pub, bool flag );
// Flags for turning on/off publishing specific components
110 bool publish_evaluation_;
111 bool publish_global_plan_;
112 bool publish_transformed_;
113 bool publish_local_plan_;
114 bool publish_trajectories_;
115 bool publish_cost_grid_pc_;
116 bool publish_input_params_;
// Marker Lifetime
119 builtin_interfaces::msg::Duration marker_lifetime_;
// Publisher Objects
std::shared_ptr<LifecyclePublisher<dwb_msgs::msg::LocalPlanEvaluation>> eval_pub_;
std::shared_ptr<LifecyclePublisher<nav_msgs::msg::Path>> global_pub_;
std::shared_ptr<LifecyclePublisher<nav_msgs::msg::Path>> transformed_pub_;
std::shared_ptr<LifecyclePublisher<nav_msgs::msg::Path>> local_pub_;
std::shared_ptr<LifecyclePublisher<visualization_msgs::msg::MarkerArray>> marker_pub_;
std::shared_ptr<LifecyclePublisher<sensor_msgs::msg::PointCloud2>> cost_grid_pc_pub_;
rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
rclcpp::Clock::SharedPtr clock_;
std::string plugin_name_;
};
} // namespace dwb_core
#endif // DWB_CORE__PUBLISHER_HPP_
/*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CORE__TRAJECTORY_CRITIC_HPP_
#define DWB_CORE__TRAJECTORY_CRITIC_HPP_
#include <string>
#include <vector>
#include <memory>
#include <utility>
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "geometry_msgs/msg/pose2_d.hpp"
#include "nav_2d_msgs/msg/twist2_d.hpp"
#include "nav_2d_msgs/msg/path2_d.hpp"
#include "dwb_msgs/msg/trajectory2_d.hpp"
#include "sensor_msgs/msg/point_cloud.hpp"
#include "nav2_util/lifecycle_node.hpp"
namespace dwb_core
{
/**
* @class TrajectoryCritic
* @brief Evaluates a Trajectory2D to produce a score
*
* This class defines the plugin interface for the TrajectoryCritic which
* gives scores to trajectories, where lower numbers are better, but negative
* scores are considered invalid.
*
* The general lifecycle is
* 1 ) initialize is called once at the beginning which in turn calls onInit.
* Derived classes may override onInit to load parameters as needed.
* 2 ) prepare is called once before each set of trajectories.
* It is presumed that there are multiple trajectories that we want to evaluate,
* and there may be some shared work that can be done beforehand to optimize
* the scoring of each individual trajectory.
* 3 ) scoreTrajectory is called once per trajectory and returns the score.
* 4 ) debrief is called after each set of trajectories with the chosen trajectory.
* This can be used for stateful critics that monitor the trajectory through time.
*
* Optionally, there is also a debugging mechanism for certain types of critics in the
* addCriticVisualization method. If the score for a trajectory depends on its relationship to
* the costmap, addCriticVisualization can provide that information to the dwb_core
* which will publish the grid scores as a PointCloud2.
*/
78 class TrajectoryCritic
{
public:
using Ptr = std::shared_ptr<dwb_core::TrajectoryCritic>;
virtual ~TrajectoryCritic( ) {}
/**
* @brief Initialize the critic with appropriate pointers and parameters
*
* The name and costmap are stored as member variables.
* A NodeHandle is created using the combination of the parent namespace and the critic name
*
* @param name The name of this critic
* @param parent_namespace The namespace of the planner
* @param costmap_ros Pointer to the costmap
*/
void initialize(
const nav2_util::LifecycleNode::SharedPtr & nh,
const std::string & name,
const std::string & ns,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros )
{
node_ = nh;
102 name_ = name;
103 costmap_ros_ = costmap_ros;
dwb_plugin_name_ = ns;
if ( !nh->has_parameter( dwb_plugin_name_ + "." + name_ + ".scale" ) ) {
nh->declare_parameter(
dwb_plugin_name_ + "." + name_ + ".scale",
rclcpp::ParameterValue( 1.0 ) );
}
nh->get_parameter( dwb_plugin_name_ + "." + name_ + ".scale", scale_ );
onInit( );
}
virtual void onInit( ) {}
/**
* @brief Reset the state of the critic
*
* Reset is called when the planner receives a new global plan.
* This can be used to discard information specific to one plan.
*/
virtual void reset( ) {}
/**
* @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
*
* Subclasses may overwrite. Return false in case there is any error.
*
* @param pose Current pose ( costmap frame )
* @param vel Current velocity
* @param goal The final goal ( costmap frame )
* @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
*/
virtual bool prepare(
const geometry_msgs::msg::Pose2D &, const nav_2d_msgs::msg::Twist2D &,
const geometry_msgs::msg::Pose2D &,
const nav_2d_msgs::msg::Path2D & )
{
return true;
}
/**
* @brief Return a raw score for the given trajectory.
*
* scores < 0 are considered invalid/errors, such as collisions
* This is the raw score in that the scale should not be applied to it.
*/
virtual double scoreTrajectory( const dwb_msgs::msg::Trajectory2D & traj ) = 0;
/**
* @brief debrief informs the critic what the chosen cmd_vel was ( if it cares )
*/
virtual void debrief( const nav_2d_msgs::msg::Twist2D & ) {}
/**
* @brief Add information to the given pointcloud for debugging costmap-grid based scores
*
* addCriticVisualization is an optional debugging mechanism for providing rich information
* about the cost for certain trajectories. Some critics will have scoring mechanisms
* wherein there will be some score for each cell in the costmap. This could be as
* straightforward as the cost in the costmap, or it could be the number of cells away
* from the goal pose.
*
* Prior to calling this, dwb_core will load the PointCloud's header and the points
* in row-major order. The critic may then add a ChannelFloat to the channels member of the PC
* with the same number of values as the points array. This information may then be converted
* and published as a PointCloud2.
*
* @param pc PointCloud to add channels to
*/
virtual void addCriticVisualization( std::vector<std::pair<std::string, std::vector<float>>> & ) {}
std::string getName( )
{
return name_;
}
virtual double getScale( ) const {return scale_;}
void setScale( const double scale ) {scale_ = scale;}
protected:
std::string name_;
std::string dwb_plugin_name_;
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
double scale_;
rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
};
} // namespace dwb_core
#endif // DWB_CORE__TRAJECTORY_CRITIC_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CORE__TRAJECTORY_GENERATOR_HPP_
#define DWB_CORE__TRAJECTORY_GENERATOR_HPP_
#include <vector>
#include <string>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "nav_2d_msgs/msg/twist2_d.hpp"
#include "dwb_msgs/msg/trajectory2_d.hpp"
#include "nav2_util/lifecycle_node.hpp"
namespace dwb_core
{
/**
* @class TrajectoryGenerator
* @brief Interface for iterating through possible velocities and creating trajectories
*
* This class defines the plugin interface for two separate but related components.
*
* First, this class provides an iterator interface for exploring all of the velocities
* to search, given the current velocity.
*
* Second, the class gives an independent interface for creating a trajectory from a twist,
* i.e. projecting it out in time and space.
*
* Both components rely heavily on the robot's kinematic model, and can share many parameters,
* which is why they are grouped into a singular class.
*/
64 class TrajectoryGenerator
{
public:
typedef std::shared_ptr<dwb_core::TrajectoryGenerator> Ptr;
69 virtual ~TrajectoryGenerator( ) {}
/**
* @brief Initialize parameters as needed
* @param nh NodeHandle to read parameters from
*/
75 virtual void initialize(
76 const nav2_util::LifecycleNode::SharedPtr & nh,
77 const std::string & plugin_name ) = 0;
78 virtual void reset( ) {}
/**
* @brief Start a new iteration based on the current velocity
* @param current_velocity
*/
83 virtual void startNewIteration( const nav_2d_msgs::msg::Twist2D & current_velocity ) = 0;
/**
* @brief Test to see whether there are more twists to test
* @return True if more twists, false otherwise
*/
89 virtual bool hasMoreTwists( ) = 0;
/**
* @brief Return the next twist and advance the iteration
* @return The Twist!
*/
95 virtual nav_2d_msgs::msg::Twist2D nextTwist( ) = 0;
/**
* @brief Get all the twists for an iteration.
*
* Note: Resets the iterator if one is in process
*
* @param current_velocity
* @return all the twists
*/
105 virtual std::vector<nav_2d_msgs::msg::Twist2D> getTwists(
106 const nav_2d_msgs::msg::Twist2D & current_velocity )
{
std::vector<nav_2d_msgs::msg::Twist2D> twists;
startNewIteration( current_velocity );
while ( hasMoreTwists( ) ) {
twists.push_back( nextTwist( ) );
}
return twists;
}
/**
* @brief Given a cmd_vel in the robot's frame and initial conditions, generate a Trajectory2D
* @param start_pose Current robot location
* @param start_vel Current robot velocity
* @param cmd_vel The desired command velocity
*/
122 virtual dwb_msgs::msg::Trajectory2D generateTrajectory(
123 const geometry_msgs::msg::Pose2D & start_pose,
124 const nav_2d_msgs::msg::Twist2D & start_vel,
125 const nav_2d_msgs::msg::Twist2D & cmd_vel ) = 0;
/**
* @brief Limits the maximum linear speed of the robot.
* @param speed_limit expressed in absolute value ( in m/s )
* or in percentage from maximum robot speed.
* @param percentage Setting speed limit in percentage if true
* or in absolute values in false case.
*/
134 virtual void setSpeedLimit( const double & speed_limit, const bool & percentage ) = 0;
};
} // namespace dwb_core
#endif // DWB_CORE__TRAJECTORY_GENERATOR_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2018, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CORE__TRAJECTORY_UTILS_HPP_
#define DWB_CORE__TRAJECTORY_UTILS_HPP_
#include "rclcpp/rclcpp.hpp"
#include "dwb_msgs/msg/trajectory2_d.hpp"
namespace dwb_core
{
/**
* @brief Helper function to find a pose in the trajectory with a particular time time_offset
* @param trajectory The trajectory to search
* @param time_offset The desired time_offset
* @return reference to the pose that is closest to the particular time offset
*
* Linearly searches through the poses. Once the poses time_offset is greater than the desired time_offset,
* the search ends, since the poses have increasing time_offsets.
*/
53 const geometry_msgs::msg::Pose2D & getClosestPose(
const dwb_msgs::msg::Trajectory2D & trajectory,
const double time_offset );
/**
* @brief Helper function to create a pose with an exact time_offset by linearly interpolating between existing poses
* @param trajectory The trajectory with pose and time offset information
* @param time_offset The desired time_offset
* @return New Pose2D with interpolated values
* @note If the given time offset is outside the bounds of the trajectory, the return pose will be either the first or last pose.
*/
64 geometry_msgs::msg::Pose2D projectPose(
const dwb_msgs::msg::Trajectory2D & trajectory,
const double time_offset );
} // namespace dwb_core
#endif // DWB_CORE__TRAJECTORY_UTILS_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <algorithm>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "dwb_core/dwb_local_planner.hpp"
#include "dwb_core/exceptions.hpp"
#include "dwb_core/illegal_trajectory_tracker.hpp"
#include "dwb_msgs/msg/critic_score.hpp"
#include "nav_2d_msgs/msg/twist2_d.hpp"
#include "nav_2d_utils/conversions.hpp"
#include "nav_2d_utils/parameters.hpp"
#include "nav_2d_utils/tf_help.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/node_utils.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "nav_msgs/msg/path.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
using nav2_util::declare_parameter_if_not_declared;
using nav2_util::geometry_utils::euclidean_distance;
namespace dwb_core
{
62 DWBLocalPlanner::DWBLocalPlanner( )
: traj_gen_loader_( "dwb_core", "dwb_core::TrajectoryGenerator" ),
critic_loader_( "dwb_core", "dwb_core::TrajectoryCritic" )
{
}
68 void DWBLocalPlanner::configure(
69 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
70 std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
71 std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros )
{
node_ = parent;
auto node = node_.lock( );
logger_ = node->get_logger( );
clock_ = node->get_clock( );
costmap_ros_ = costmap_ros;
tf_ = tf;
dwb_plugin_name_ = name;
declare_parameter_if_not_declared(
node, dwb_plugin_name_ + ".critics",
rclcpp::PARAMETER_STRING_ARRAY );
declare_parameter_if_not_declared(
node, dwb_plugin_name_ + ".default_critic_namespaces",
rclcpp::ParameterValue( std::vector<std::string>( ) ) );
declare_parameter_if_not_declared(
node, dwb_plugin_name_ + ".prune_plan",
rclcpp::ParameterValue( true ) );
declare_parameter_if_not_declared(
node, dwb_plugin_name_ + ".prune_distance",
rclcpp::ParameterValue( 2.0 ) );
declare_parameter_if_not_declared(
node, dwb_plugin_name_ + ".debug_trajectory_details",
rclcpp::ParameterValue( false ) );
declare_parameter_if_not_declared(
node, dwb_plugin_name_ + ".trajectory_generator_name",
rclcpp::ParameterValue( std::string( "dwb_plugins::StandardTrajectoryGenerator" ) ) );
declare_parameter_if_not_declared(
node, dwb_plugin_name_ + ".transform_tolerance",
rclcpp::ParameterValue( 0.1 ) );
declare_parameter_if_not_declared(
node, dwb_plugin_name_ + ".shorten_transformed_plan",
rclcpp::ParameterValue( true ) );
declare_parameter_if_not_declared(
node, dwb_plugin_name_ + ".short_circuit_trajectory_evaluation",
rclcpp::ParameterValue( true ) );
std::string traj_generator_name;
double transform_tolerance;
node->get_parameter( dwb_plugin_name_ + ".transform_tolerance", transform_tolerance );
transform_tolerance_ = rclcpp::Duration::from_seconds( transform_tolerance );
RCLCPP_INFO( logger_, "Setting transform_tolerance to %f", transform_tolerance );
node->get_parameter( dwb_plugin_name_ + ".prune_plan", prune_plan_ );
node->get_parameter( dwb_plugin_name_ + ".prune_distance", prune_distance_ );
node->get_parameter( dwb_plugin_name_ + ".debug_trajectory_details", debug_trajectory_details_ );
node->get_parameter( dwb_plugin_name_ + ".trajectory_generator_name", traj_generator_name );
node->get_parameter(
dwb_plugin_name_ + ".short_circuit_trajectory_evaluation",
short_circuit_trajectory_evaluation_ );
node->get_parameter( dwb_plugin_name_ + ".shorten_transformed_plan", shorten_transformed_plan_ );
pub_ = std::make_unique<DWBPublisher>( node, dwb_plugin_name_ );
pub_->on_configure( );
traj_generator_ = traj_gen_loader_.createUniqueInstance( traj_generator_name );
traj_generator_->initialize( node, dwb_plugin_name_ );
try {
loadCritics( );
} catch ( const std::exception & e ) {
RCLCPP_ERROR( logger_, "Couldn't load critics! Caught exception: %s", e.what( ) );
throw;
}
}
void
141 DWBLocalPlanner::activate( )
{
pub_->on_activate( );
}
void
147 DWBLocalPlanner::deactivate( )
{
pub_->on_deactivate( );
}
void
153 DWBLocalPlanner::cleanup( )
{
pub_->on_cleanup( );
traj_generator_.reset( );
}
std::string
161 DWBLocalPlanner::resolveCriticClassName( std::string base_name )
{
if ( base_name.find( "Critic" ) == std::string::npos ) {
base_name = base_name + "Critic";
}
if ( base_name.find( "::" ) == std::string::npos ) {
for ( unsigned int j = 0; j < default_critic_namespaces_.size( ); j++ ) {
std::string full_name = default_critic_namespaces_[j] + "::" + base_name;
if ( critic_loader_.isClassAvailable( full_name ) ) {
return full_name;
}
}
}
return base_name;
}
void
179 DWBLocalPlanner::loadCritics( )
{
auto node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
node->get_parameter( dwb_plugin_name_ + ".default_critic_namespaces", default_critic_namespaces_ );
if ( default_critic_namespaces_.empty( ) ) {
default_critic_namespaces_.emplace_back( "dwb_critics" );
}
std::vector<std::string> critic_names;
if ( !node->get_parameter( dwb_plugin_name_ + ".critics", critic_names ) ) {
throw std::runtime_error( "No critics defined for " + dwb_plugin_name_ );
}
node->get_parameter( dwb_plugin_name_ + ".critics", critic_names );
for ( unsigned int i = 0; i < critic_names.size( ); i++ ) {
std::string critic_plugin_name = critic_names[i];
std::string plugin_class;
declare_parameter_if_not_declared(
node, dwb_plugin_name_ + "." + critic_plugin_name + ".class",
rclcpp::ParameterValue( critic_plugin_name ) );
node->get_parameter( dwb_plugin_name_ + "." + critic_plugin_name + ".class", plugin_class );
plugin_class = resolveCriticClassName( plugin_class );
TrajectoryCritic::Ptr plugin = critic_loader_.createUniqueInstance( plugin_class );
RCLCPP_INFO(
logger_,
"Using critic \"%s\" ( %s )", critic_plugin_name.c_str( ), plugin_class.c_str( ) );
critics_.push_back( plugin );
try {
plugin->initialize( node, critic_plugin_name, dwb_plugin_name_, costmap_ros_ );
} catch ( const std::exception & e ) {
RCLCPP_ERROR( logger_, "Couldn't initialize critic plugin!" );
throw;
}
RCLCPP_INFO( logger_, "Critic plugin initialized" );
}
}
void
224 DWBLocalPlanner::setPlan( const nav_msgs::msg::Path & path )
{
auto path2d = nav_2d_utils::pathToPath2D( path );
for ( TrajectoryCritic::Ptr & critic : critics_ ) {
critic->reset( );
}
traj_generator_->reset( );
pub_->publishGlobalPlan( path2d );
global_plan_ = path2d;
}
geometry_msgs::msg::TwistStamped
238 DWBLocalPlanner::computeVelocityCommands(
239 const geometry_msgs::msg::PoseStamped & pose,
240 const geometry_msgs::msg::Twist & velocity,
241 nav2_core::GoalChecker * /*goal_checker*/ )
{
std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> results = nullptr;
if ( pub_->shouldRecordEvaluation( ) ) {
results = std::make_shared<dwb_msgs::msg::LocalPlanEvaluation>( );
}
try {
nav_2d_msgs::msg::Twist2DStamped cmd_vel2d = computeVelocityCommands(
nav_2d_utils::poseStampedToPose2D( pose ),
nav_2d_utils::twist3Dto2D( velocity ), results );
pub_->publishEvaluation( results );
geometry_msgs::msg::TwistStamped cmd_vel;
cmd_vel.twist = nav_2d_utils::twist2Dto3D( cmd_vel2d.velocity );
return cmd_vel;
} catch ( const nav2_core::PlannerException & e ) {
pub_->publishEvaluation( results );
throw;
}
}
void
263 DWBLocalPlanner::prepareGlobalPlan(
264 const nav_2d_msgs::msg::Pose2DStamped & pose, nav_2d_msgs::msg::Path2D & transformed_plan,
265 nav_2d_msgs::msg::Pose2DStamped & goal_pose, bool publish_plan )
{
transformed_plan = transformGlobalPlan( pose );
if ( publish_plan ) {
pub_->publishTransformedPlan( transformed_plan );
}
goal_pose.header.frame_id = global_plan_.header.frame_id;
goal_pose.pose = global_plan_.poses.back( );
nav_2d_utils::transformPose(
tf_, costmap_ros_->getGlobalFrameID( ), goal_pose,
goal_pose, transform_tolerance_ );
}
nav_2d_msgs::msg::Twist2DStamped
280 DWBLocalPlanner::computeVelocityCommands(
281 const nav_2d_msgs::msg::Pose2DStamped & pose,
282 const nav_2d_msgs::msg::Twist2D & velocity,
283 std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> & results )
{
if ( results ) {
results->header.frame_id = pose.header.frame_id;
results->header.stamp = clock_->now( );
}
nav_2d_msgs::msg::Path2D transformed_plan;
nav_2d_msgs::msg::Pose2DStamped goal_pose;
prepareGlobalPlan( pose, transformed_plan, goal_pose );
nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap( );
std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock( *( costmap->getMutex( ) ) );
for ( TrajectoryCritic::Ptr & critic : critics_ ) {
if ( !critic->prepare( pose.pose, velocity, goal_pose.pose, transformed_plan ) ) {
RCLCPP_WARN( rclcpp::get_logger( "DWBLocalPlanner" ), "A scoring function failed to prepare" );
}
}
try {
dwb_msgs::msg::TrajectoryScore best = coreScoringAlgorithm( pose.pose, velocity, results );
// Return Value
nav_2d_msgs::msg::Twist2DStamped cmd_vel;
cmd_vel.header.stamp = clock_->now( );
cmd_vel.velocity = best.traj.velocity;
// debrief stateful scoring functions
for ( TrajectoryCritic::Ptr & critic : critics_ ) {
critic->debrief( cmd_vel.velocity );
}
lock.unlock( );
pub_->publishLocalPlan( pose.header, best.traj );
pub_->publishCostGrid( costmap_ros_, critics_ );
return cmd_vel;
} catch ( const dwb_core::NoLegalTrajectoriesException & e ) {
nav_2d_msgs::msg::Twist2D empty_cmd;
dwb_msgs::msg::Trajectory2D empty_traj;
// debrief stateful scoring functions
for ( TrajectoryCritic::Ptr & critic : critics_ ) {
critic->debrief( empty_cmd );
}
lock.unlock( );
pub_->publishLocalPlan( pose.header, empty_traj );
pub_->publishCostGrid( costmap_ros_, critics_ );
throw;
}
}
dwb_msgs::msg::TrajectoryScore
341 DWBLocalPlanner::coreScoringAlgorithm(
342 const geometry_msgs::msg::Pose2D & pose,
343 const nav_2d_msgs::msg::Twist2D velocity,
344 std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> & results )
{
nav_2d_msgs::msg::Twist2D twist;
dwb_msgs::msg::Trajectory2D traj;
dwb_msgs::msg::TrajectoryScore best, worst;
best.total = -1;
worst.total = -1;
IllegalTrajectoryTracker tracker;
traj_generator_->startNewIteration( velocity );
while ( traj_generator_->hasMoreTwists( ) ) {
twist = traj_generator_->nextTwist( );
traj = traj_generator_->generateTrajectory( pose, velocity, twist );
try {
dwb_msgs::msg::TrajectoryScore score = scoreTrajectory( traj, best.total );
tracker.addLegalTrajectory( );
if ( results ) {
results->twists.push_back( score );
}
if ( best.total < 0 || score.total < best.total ) {
best = score;
if ( results ) {
results->best_index = results->twists.size( ) - 1;
}
}
if ( worst.total < 0 || score.total > worst.total ) {
worst = score;
if ( results ) {
results->worst_index = results->twists.size( ) - 1;
}
}
} catch ( const dwb_core::IllegalTrajectoryException & e ) {
if ( results ) {
dwb_msgs::msg::TrajectoryScore failed_score;
failed_score.traj = traj;
dwb_msgs::msg::CriticScore cs;
cs.name = e.getCriticName( );
cs.raw_score = -1.0;
failed_score.scores.push_back( cs );
failed_score.total = -1.0;
results->twists.push_back( failed_score );
}
tracker.addIllegalTrajectory( e );
}
}
if ( best.total < 0 ) {
if ( debug_trajectory_details_ ) {
RCLCPP_ERROR( rclcpp::get_logger( "DWBLocalPlanner" ), "%s", tracker.getMessage( ).c_str( ) );
for ( auto const & x : tracker.getPercentages( ) ) {
RCLCPP_ERROR(
rclcpp::get_logger(
"DWBLocalPlanner" ), "%.2f: %10s/%s", x.second,
x.first.first.c_str( ), x.first.second.c_str( ) );
}
}
throw NoLegalTrajectoriesException( tracker );
}
return best;
}
dwb_msgs::msg::TrajectoryScore
409 DWBLocalPlanner::scoreTrajectory(
410 const dwb_msgs::msg::Trajectory2D & traj,
double best_score )
{
dwb_msgs::msg::TrajectoryScore score;
score.traj = traj;
for ( TrajectoryCritic::Ptr & critic : critics_ ) {
dwb_msgs::msg::CriticScore cs;
cs.name = critic->getName( );
cs.scale = critic->getScale( );
if ( cs.scale == 0.0 ) {
score.scores.push_back( cs );
continue;
}
double critic_score = critic->scoreTrajectory( traj );
cs.raw_score = critic_score;
score.scores.push_back( cs );
score.total += critic_score * cs.scale;
if ( short_circuit_trajectory_evaluation_ && best_score > 0 && score.total > best_score ) {
// since we keep adding positives, once we are worse than the best, we will stay worse
break;
}
}
return score;
}
nav_2d_msgs::msg::Path2D
440 DWBLocalPlanner::transformGlobalPlan(
441 const nav_2d_msgs::msg::Pose2DStamped & pose )
{
if ( global_plan_.poses.empty( ) ) {
throw nav2_core::PlannerException( "Received plan with zero length" );
}
// let's get the pose of the robot in the frame of the plan
nav_2d_msgs::msg::Pose2DStamped robot_pose;
if ( !nav_2d_utils::transformPose(
tf_, global_plan_.header.frame_id, pose,
robot_pose, transform_tolerance_ ) )
{
throw dwb_core::
PlannerTFException( "Unable to transform robot pose into global plan's frame" );
}
// we'll discard points on the plan that are outside the local costmap
nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap( );
double dist_threshold = std::max( costmap->getSizeInCellsX( ), costmap->getSizeInCellsY( ) ) *
costmap->getResolution( ) / 2.0;
// If prune_plan is enabled ( it is by default ) then we want to restrict the
// plan to distances within that range as well.
double prune_dist = prune_distance_;
// Set the maximum distance we'll include points before getting to the part
// of the path where the robot is located ( the start of the plan ). Basically,
// these are the points the robot has already passed.
double transform_start_threshold;
if ( prune_plan_ ) {
transform_start_threshold = std::min( dist_threshold, prune_dist );
} else {
transform_start_threshold = dist_threshold;
}
// Set the maximum distance we'll include points after the part of the part of
// the plan near the robot ( the end of the plan ). This determines the amount
// of the plan passed on to the critics
double transform_end_threshold;
if ( shorten_transformed_plan_ ) {
transform_end_threshold = std::min( dist_threshold, prune_dist );
} else {
transform_end_threshold = dist_threshold;
}
// Find the first pose in the global plan that's further than prune distance
// from the robot using integrated distance
auto prune_point = nav2_util::geometry_utils::first_after_integrated_distance(
global_plan_.poses.begin( ), global_plan_.poses.end( ), prune_dist );
// Find the first pose in the plan ( upto prune_point ) that's less than transform_start_threshold
// from the robot.
auto transformation_begin = std::find_if(
begin( global_plan_.poses ), prune_point,
[&]( const auto & global_plan_pose ) {
return euclidean_distance( robot_pose.pose, global_plan_pose ) < transform_start_threshold;
} );
// Find the first pose in the end of the plan that's further than transform_end_threshold
// from the robot using integrated distance
auto transformation_end = std::find_if(
transformation_begin, global_plan_.poses.end( ),
[&]( const auto & pose ) {
return euclidean_distance( pose, robot_pose.pose ) > transform_end_threshold;
} );
// Transform the near part of the global plan into the robot's frame of reference.
nav_2d_msgs::msg::Path2D transformed_plan;
transformed_plan.header.frame_id = costmap_ros_->getGlobalFrameID( );
transformed_plan.header.stamp = pose.header.stamp;
// Helper function for the transform below. Converts a pose2D from global
// frame to local
auto transformGlobalPoseToLocal = [&]( const auto & global_plan_pose ) {
nav_2d_msgs::msg::Pose2DStamped stamped_pose, transformed_pose;
stamped_pose.header.frame_id = global_plan_.header.frame_id;
stamped_pose.pose = global_plan_pose;
nav_2d_utils::transformPose(
tf_, transformed_plan.header.frame_id,
stamped_pose, transformed_pose, transform_tolerance_ );
return transformed_pose.pose;
};
std::transform(
transformation_begin, transformation_end,
std::back_inserter( transformed_plan.poses ),
transformGlobalPoseToLocal );
// Remove the portion of the global plan that we've already passed so we don't
// process it on the next iteration.
if ( prune_plan_ ) {
global_plan_.poses.erase( begin( global_plan_.poses ), transformation_begin );
pub_->publishGlobalPlan( global_plan_ );
}
if ( transformed_plan.poses.empty( ) ) {
throw nav2_core::PlannerException( "Resulting plan has 0 poses in it." );
}
return transformed_plan;
}
} // namespace dwb_core
// Register this controller as a nav2_core plugin
546 PLUGINLIB_EXPORT_CLASS(
dwb_core::DWBLocalPlanner,
nav2_core::Controller )
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "dwb_core/illegal_trajectory_tracker.hpp"
#include <map>
#include <utility>
#include <string>
#include <sstream>
namespace dwb_core
{
43 void IllegalTrajectoryTracker::addIllegalTrajectory(
44 const dwb_core::IllegalTrajectoryException & e )
{
counts_[std::make_pair( e.getCriticName( ), e.what( ) )]++;
illegal_count_++;
}
50 void IllegalTrajectoryTracker::addLegalTrajectory( )
{
legal_count_++;
}
std::map<std::pair<std::string, std::string>,
56 double> IllegalTrajectoryTracker::getPercentages( ) const
{
std::map<std::pair<std::string, std::string>, double> percents;
double denominator = static_cast<double>( legal_count_ + illegal_count_ );
for ( auto const & x : counts_ ) {
percents[x.first] = static_cast<double>( x.second ) / denominator;
}
return percents;
}
66 std::string IllegalTrajectoryTracker::getMessage( ) const
{
std::ostringstream msg;
if ( legal_count_ == 0 ) {
msg << "No valid trajectories out of " << illegal_count_ << "! ";
} else {
unsigned int total = legal_count_ + illegal_count_;
msg << legal_count_ << " valid trajectories found ( ";
msg << static_cast<double>( 100 * legal_count_ ) / static_cast<double>( total );
msg << "% of " << total << " ). ";
}
return msg.str( );
}
} // namespace dwb_core
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "dwb_core/publisher.hpp"
#include <algorithm>
#include <memory>
#include <string>
#include <vector>
#include <utility>
#include "sensor_msgs/point_cloud2_iterator.hpp"
#include "nav_2d_utils/conversions.hpp"
#include "nav2_util/node_utils.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "visualization_msgs/msg/marker_array.hpp"
#include "visualization_msgs/msg/marker.hpp"
using std::max;
using std::string;
using nav2_util::declare_parameter_if_not_declared;
namespace dwb_core
{
57 DWBPublisher::DWBPublisher(
58 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
59 const std::string & plugin_name )
: node_( parent ),
plugin_name_( plugin_name )
{
auto node = node_.lock( );
clock_ = node->get_clock( );
}
nav2_util::CallbackReturn
68 DWBPublisher::on_configure( )
{
auto node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
declare_parameter_if_not_declared(
node, plugin_name_ + ".publish_evaluation",
rclcpp::ParameterValue( true ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".publish_global_plan",
rclcpp::ParameterValue( true ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".publish_transformed_plan",
rclcpp::ParameterValue( true ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".publish_local_plan",
rclcpp::ParameterValue( true ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".publish_trajectories",
rclcpp::ParameterValue( true ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".publish_cost_grid_pc",
rclcpp::ParameterValue( false ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".marker_lifetime",
rclcpp::ParameterValue( 0.1 ) );
node->get_parameter( plugin_name_ + ".publish_evaluation", publish_evaluation_ );
node->get_parameter( plugin_name_ + ".publish_global_plan", publish_global_plan_ );
node->get_parameter( plugin_name_ + ".publish_transformed_plan", publish_transformed_ );
node->get_parameter( plugin_name_ + ".publish_local_plan", publish_local_plan_ );
node->get_parameter( plugin_name_ + ".publish_trajectories", publish_trajectories_ );
node->get_parameter( plugin_name_ + ".publish_cost_grid_pc", publish_cost_grid_pc_ );
eval_pub_ = node->create_publisher<dwb_msgs::msg::LocalPlanEvaluation>( "evaluation", 1 );
global_pub_ = node->create_publisher<nav_msgs::msg::Path>( "received_global_plan", 1 );
transformed_pub_ = node->create_publisher<nav_msgs::msg::Path>( "transformed_global_plan", 1 );
local_pub_ = node->create_publisher<nav_msgs::msg::Path>( "local_plan", 1 );
marker_pub_ = node->create_publisher<visualization_msgs::msg::MarkerArray>( "marker", 1 );
cost_grid_pc_pub_ = node->create_publisher<sensor_msgs::msg::PointCloud2>( "cost_cloud", 1 );
double marker_lifetime = 0.0;
node->get_parameter( plugin_name_ + ".marker_lifetime", marker_lifetime );
marker_lifetime_ = rclcpp::Duration::from_seconds( marker_lifetime );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
119 DWBPublisher::on_activate( )
{
eval_pub_->on_activate( );
global_pub_->on_activate( );
transformed_pub_->on_activate( );
local_pub_->on_activate( );
marker_pub_->on_activate( );
cost_grid_pc_pub_->on_activate( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
132 DWBPublisher::on_deactivate( )
{
eval_pub_->on_deactivate( );
global_pub_->on_deactivate( );
transformed_pub_->on_deactivate( );
local_pub_->on_deactivate( );
marker_pub_->on_deactivate( );
cost_grid_pc_pub_->on_deactivate( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
145 DWBPublisher::on_cleanup( )
{
eval_pub_.reset( );
global_pub_.reset( );
transformed_pub_.reset( );
local_pub_.reset( );
marker_pub_.reset( );
cost_grid_pc_pub_.reset( );
return nav2_util::CallbackReturn::SUCCESS;
}
void
158 DWBPublisher::publishEvaluation( std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> results )
{
if ( results ) {
if ( publish_evaluation_ && eval_pub_->get_subscription_count( ) > 0 ) {
auto msg = std::make_unique<dwb_msgs::msg::LocalPlanEvaluation>( *results );
eval_pub_->publish( std::move( msg ) );
}
publishTrajectories( *results );
}
}
void
170 DWBPublisher::publishTrajectories( const dwb_msgs::msg::LocalPlanEvaluation & results )
{
if ( marker_pub_->get_subscription_count( ) < 1 ) {return;}
if ( !publish_trajectories_ ) {return;}
auto ma = std::make_unique<visualization_msgs::msg::MarkerArray>( );
visualization_msgs::msg::Marker m;
if ( results.twists.size( ) == 0 ) {return;}
geometry_msgs::msg::Point pt;
m.header = results.header;
m.type = m.LINE_STRIP;
m.pose.orientation.w = 1;
m.scale.x = 0.002;
m.color.a = 1.0;
m.lifetime = marker_lifetime_;
double best_cost = results.twists[results.best_index].total;
double worst_cost = results.twists[results.worst_index].total;
double denominator = worst_cost - best_cost;
if ( std::fabs( denominator ) < 1e-9 ) {
denominator = 1.0;
}
unsigned currentValidId = 0;
unsigned currentInvalidId = 0;
string validNamespace( "ValidTrajectories" );
string invalidNamespace( "InvalidTrajectories" );
for ( unsigned int i = 0; i < results.twists.size( ); i++ ) {
const dwb_msgs::msg::TrajectoryScore & twist = results.twists[i];
double displayLevel = ( twist.total - best_cost ) / denominator;
if ( twist.total >= 0 ) {
m.color.r = displayLevel;
m.color.g = 1.0 - displayLevel;
m.color.b = 0;
m.color.a = 1.0;
m.ns = validNamespace;
m.id = currentValidId;
++currentValidId;
} else {
m.color.r = 0;
m.color.g = 0;
m.color.b = 0;
m.color.a = 1.0;
m.ns = invalidNamespace;
m.id = currentInvalidId;
++currentInvalidId;
}
m.points.clear( );
for ( unsigned int j = 0; j < twist.traj.poses.size( ); ++j ) {
pt.x = twist.traj.poses[j].x;
pt.y = twist.traj.poses[j].y;
pt.z = 0;
m.points.push_back( pt );
}
ma->markers.push_back( m );
}
marker_pub_->publish( std::move( ma ) );
}
void
234 DWBPublisher::publishLocalPlan(
235 const std_msgs::msg::Header & header,
236 const dwb_msgs::msg::Trajectory2D & traj )
{
if ( !publish_local_plan_ ) {return;}
auto path =
std::make_unique<nav_msgs::msg::Path>(
nav_2d_utils::poses2DToPath(
traj.poses, header.frame_id,
header.stamp ) );
if ( local_pub_->get_subscription_count( ) > 0 ) {
local_pub_->publish( std::move( path ) );
}
}
void
252 DWBPublisher::publishCostGrid(
253 const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,
254 const std::vector<TrajectoryCritic::Ptr> critics )
{
if ( cost_grid_pc_pub_->get_subscription_count( ) < 1 ) {return;}
if ( !publish_cost_grid_pc_ ) {return;}
auto cost_grid_pc = std::make_unique<sensor_msgs::msg::PointCloud2>( );
cost_grid_pc->header.frame_id = costmap_ros->getGlobalFrameID( );
cost_grid_pc->header.stamp = clock_->now( );
nav2_costmap_2d::Costmap2D * costmap = costmap_ros->getCostmap( );
double x_coord, y_coord;
unsigned int size_x = costmap->getSizeInCellsX( );
unsigned int size_y = costmap->getSizeInCellsY( );
std::vector<std::pair<std::string, std::vector<float>>> cost_channels;
std::vector<float> total_cost( size_x * size_y, 0.0 );
for ( TrajectoryCritic::Ptr critic : critics ) {
unsigned int channel_index = cost_channels.size( );
critic->addCriticVisualization( cost_channels );
if ( channel_index == cost_channels.size( ) ) {
// No channels were added, so skip to next critic
continue;
}
double scale = critic->getScale( );
for ( unsigned int i = 0; i < size_x * size_y; i++ ) {
total_cost[i] += cost_channels[channel_index].second[i] * scale;
}
}
cost_channels.push_back( std::make_pair( "total_cost", total_cost ) );
cost_grid_pc->width = size_x * size_y;
cost_grid_pc->height = 1;
cost_grid_pc->fields.resize( 3 + cost_channels.size( ) ); // x, y, z, + cost channels
cost_grid_pc->is_dense = true;
cost_grid_pc->is_bigendian = false;
int offset = 0;
for ( size_t i = 0; i < cost_grid_pc->fields.size( ); ++i, offset += 4 ) {
cost_grid_pc->fields[i].offset = offset;
cost_grid_pc->fields[i].count = 1;
cost_grid_pc->fields[i].datatype = sensor_msgs::msg::PointField::FLOAT32;
if ( i >= 3 ) {
cost_grid_pc->fields[i].name = cost_channels[i - 3].first;
}
}
cost_grid_pc->fields[0].name = "x";
cost_grid_pc->fields[1].name = "y";
cost_grid_pc->fields[2].name = "z";
cost_grid_pc->point_step = offset;
cost_grid_pc->row_step = cost_grid_pc->point_step * cost_grid_pc->width;
cost_grid_pc->data.resize( cost_grid_pc->row_step * cost_grid_pc->height );
std::vector<sensor_msgs::PointCloud2Iterator<float>> cost_grid_pc_iter;
for ( size_t i = 0; i < cost_grid_pc->fields.size( ); ++i ) {
sensor_msgs::PointCloud2Iterator<float> iter( *cost_grid_pc, cost_grid_pc->fields[i].name );
cost_grid_pc_iter.push_back( iter );
}
unsigned int j = 0;
for ( unsigned int cy = 0; cy < size_y; cy++ ) {
for ( unsigned int cx = 0; cx < size_x; cx++ ) {
costmap->mapToWorld( cx, cy, x_coord, y_coord );
*cost_grid_pc_iter[0] = x_coord;
*cost_grid_pc_iter[1] = y_coord;
*cost_grid_pc_iter[2] = 0.0; // z value
for ( size_t i = 3; i < cost_grid_pc_iter.size( ); ++i ) {
*cost_grid_pc_iter[i] = cost_channels[i - 3].second[j];
++cost_grid_pc_iter[i];
}
++cost_grid_pc_iter[0];
++cost_grid_pc_iter[1];
++cost_grid_pc_iter[2];
j++;
}
}
cost_grid_pc_pub_->publish( std::move( cost_grid_pc ) );
}
void
341 DWBPublisher::publishGlobalPlan( const nav_2d_msgs::msg::Path2D plan )
{
publishGenericPlan( plan, *global_pub_, publish_global_plan_ );
}
void
347 DWBPublisher::publishTransformedPlan( const nav_2d_msgs::msg::Path2D plan )
{
publishGenericPlan( plan, *transformed_pub_, publish_transformed_ );
}
void
353 DWBPublisher::publishLocalPlan( const nav_2d_msgs::msg::Path2D plan )
{
publishGenericPlan( plan, *local_pub_, publish_local_plan_ );
}
void
359 DWBPublisher::publishGenericPlan(
360 const nav_2d_msgs::msg::Path2D plan,
361 rclcpp::Publisher<nav_msgs::msg::Path> & pub, bool flag )
{
if ( pub.get_subscription_count( ) < 1 ) {return;}
if ( !flag ) {return;}
auto path = std::make_unique<nav_msgs::msg::Path>( nav_2d_utils::pathToPath( plan ) );
pub.publish( std::move( path ) );
}
} // namespace dwb_core
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2018, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <dwb_core/trajectory_utils.hpp>
#include <cmath>
#include "rclcpp/duration.hpp"
#include "dwb_core/exceptions.hpp"
namespace dwb_core
{
45 const geometry_msgs::msg::Pose2D & getClosestPose(
const dwb_msgs::msg::Trajectory2D & trajectory,
const double time_offset )
{
rclcpp::Duration goal_time = rclcpp::Duration::from_seconds( time_offset );
const unsigned int num_poses = trajectory.poses.size( );
if ( num_poses == 0 ) {
throw nav2_core::PlannerException( "Cannot call getClosestPose on empty trajectory." );
}
unsigned int closest_index = num_poses;
double closest_diff = 0.0;
for ( unsigned int i = 0; i < num_poses; ++i ) {
double diff = std::fabs( ( rclcpp::Duration( trajectory.time_offsets[i] ) - goal_time ).seconds( ) );
if ( closest_index == num_poses || diff < closest_diff ) {
closest_index = i;
closest_diff = diff;
}
if ( goal_time < rclcpp::Duration( trajectory.time_offsets[i] ) ) {
break;
}
}
return trajectory.poses[closest_index];
}
69 geometry_msgs::msg::Pose2D projectPose(
const dwb_msgs::msg::Trajectory2D & trajectory,
const double time_offset )
{
rclcpp::Duration goal_time = rclcpp::Duration::from_seconds( time_offset );
const unsigned int num_poses = trajectory.poses.size( );
if ( num_poses == 0 ) {
throw nav2_core::PlannerException( "Cannot call projectPose on empty trajectory." );
}
if ( goal_time <= ( trajectory.time_offsets[0] ) ) {
return trajectory.poses[0];
} else if ( goal_time >= rclcpp::Duration( trajectory.time_offsets[num_poses - 1] ) ) {
return trajectory.poses[num_poses - 1];
}
for ( unsigned int i = 0; i < num_poses - 1; ++i ) {
if ( goal_time >= rclcpp::Duration( trajectory.time_offsets[i] ) &&
goal_time < rclcpp::Duration( trajectory.time_offsets[i + 1] ) )
{
double time_diff =
( rclcpp::Duration( trajectory.time_offsets[i + 1] ) -
rclcpp::Duration( trajectory.time_offsets[i] ) ).seconds( );
double ratio = ( goal_time - rclcpp::Duration( trajectory.time_offsets[i] ) ).seconds( ) /
time_diff;
double inv_ratio = 1.0 - ratio;
const geometry_msgs::msg::Pose2D & pose_a = trajectory.poses[i];
const geometry_msgs::msg::Pose2D & pose_b = trajectory.poses[i + 1];
geometry_msgs::msg::Pose2D projected;
projected.x = pose_a.x * inv_ratio + pose_b.x * ratio;
projected.y = pose_a.y * inv_ratio + pose_b.y * ratio;
projected.theta = pose_a.theta * inv_ratio + pose_b.theta * ratio;
return projected;
}
}
// Should not reach this point
return trajectory.poses[num_poses - 1];
}
} // namespace dwb_core
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2018, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "gtest/gtest.h"
#include "dwb_core/trajectory_utils.hpp"
using dwb_core::getClosestPose;
using dwb_core::projectPose;
41 TEST( Utils, ClosestPose )
{
dwb_msgs::msg::Trajectory2D traj;
traj.poses.resize( 4 );
traj.time_offsets.resize( 4 );
for ( unsigned int i = 0; i < traj.poses.size( ); i++ ) {
double d = static_cast<double>( i );
traj.poses[i].x = d;
traj.time_offsets[i] = rclcpp::Duration::from_seconds( d );
}
EXPECT_DOUBLE_EQ( getClosestPose( traj, 0.0 ).x, traj.poses[0].x );
EXPECT_DOUBLE_EQ( getClosestPose( traj, -1.0 ).x, traj.poses[0].x );
EXPECT_DOUBLE_EQ( getClosestPose( traj, 0.4 ).x, traj.poses[0].x );
EXPECT_DOUBLE_EQ( getClosestPose( traj, 0.5 ).x, traj.poses[0].x );
EXPECT_DOUBLE_EQ( getClosestPose( traj, 0.51 ).x, traj.poses[1].x );
EXPECT_DOUBLE_EQ( getClosestPose( traj, 1.0 ).x, traj.poses[1].x );
EXPECT_DOUBLE_EQ( getClosestPose( traj, 1.4999 ).x, traj.poses[1].x );
EXPECT_DOUBLE_EQ( getClosestPose( traj, 2.0 ).x, traj.poses[2].x );
EXPECT_DOUBLE_EQ( getClosestPose( traj, 2.51 ).x, traj.poses[3].x );
EXPECT_DOUBLE_EQ( getClosestPose( traj, 3.5 ).x, traj.poses[3].x );
}
64 TEST( Utils, ProjectPose )
{
dwb_msgs::msg::Trajectory2D traj;
traj.poses.resize( 4 );
traj.time_offsets.resize( 4 );
for ( unsigned int i = 0; i < traj.poses.size( ); i++ ) {
double d = static_cast<double>( i );
traj.poses[i].x = d;
traj.poses[i].y = 30.0 - 2.0 * d;
traj.poses[i].theta = 0.42;
traj.time_offsets[i] = rclcpp::Duration::from_seconds( d );
}
EXPECT_DOUBLE_EQ( projectPose( traj, 0.0 ).x, 0.0 );
EXPECT_DOUBLE_EQ( projectPose( traj, 0.0 ).y, 30.0 );
EXPECT_DOUBLE_EQ( projectPose( traj, 0.0 ).theta, 0.42 );
EXPECT_DOUBLE_EQ( projectPose( traj, -1.0 ).x, 0.0 );
EXPECT_DOUBLE_EQ( projectPose( traj, -1.0 ).y, 30.0 );
EXPECT_DOUBLE_EQ( projectPose( traj, -1.0 ).theta, 0.42 );
EXPECT_DOUBLE_EQ( projectPose( traj, 0.4 ).x, 0.4 );
EXPECT_DOUBLE_EQ( projectPose( traj, 0.4 ).y, 29.2 );
EXPECT_DOUBLE_EQ( projectPose( traj, 0.4 ).theta, 0.42 );
EXPECT_DOUBLE_EQ( projectPose( traj, 0.5 ).x, 0.5 );
EXPECT_DOUBLE_EQ( projectPose( traj, 0.5 ).y, 29.0 );
EXPECT_DOUBLE_EQ( projectPose( traj, 0.5 ).theta, 0.42 );
EXPECT_DOUBLE_EQ( projectPose( traj, 0.51 ).x, 0.51 );
EXPECT_DOUBLE_EQ( projectPose( traj, 0.51 ).y, 28.98 );
EXPECT_DOUBLE_EQ( projectPose( traj, 0.51 ).theta, 0.42 );
EXPECT_DOUBLE_EQ( projectPose( traj, 1.0 ).x, 1.0 );
EXPECT_DOUBLE_EQ( projectPose( traj, 1.0 ).y, 28.0 );
EXPECT_DOUBLE_EQ( projectPose( traj, 1.0 ).theta, 0.42 );
EXPECT_DOUBLE_EQ( projectPose( traj, 1.4999 ).x, 1.4999 );
EXPECT_DOUBLE_EQ( projectPose( traj, 1.4999 ).y, 27.0002 );
EXPECT_DOUBLE_EQ( projectPose( traj, 1.4999 ).theta, 0.42 );
EXPECT_DOUBLE_EQ( projectPose( traj, 2.0 ).x, 2.0 );
EXPECT_DOUBLE_EQ( projectPose( traj, 2.0 ).y, 26.0 );
EXPECT_DOUBLE_EQ( projectPose( traj, 2.0 ).theta, 0.42 );
EXPECT_FLOAT_EQ( projectPose( traj, 2.51 ).x, 2.51 );
EXPECT_FLOAT_EQ( projectPose( traj, 2.51 ).y, 24.98 );
EXPECT_DOUBLE_EQ( projectPose( traj, 2.51 ).theta, 0.42 );
EXPECT_DOUBLE_EQ( projectPose( traj, 3.5 ).x, 3.0 );
EXPECT_DOUBLE_EQ( projectPose( traj, 3.5 ).y, 24.0 );
EXPECT_DOUBLE_EQ( projectPose( traj, 3.5 ).theta, 0.42 );
}
109 int main( int argc, char ** argv )
{
rclcpp::init( argc, argv );
testing::InitGoogleTest( &argc, argv );
return RUN_ALL_TESTS( );
}
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CRITICS__ALIGNMENT_UTIL_HPP_
#define DWB_CRITICS__ALIGNMENT_UTIL_HPP_
#include "geometry_msgs/msg/pose2_d.hpp"
namespace dwb_critics
{
/**
* @brief Projects the given pose forward the specified distance in the x direction.
* @param pose Input pose
* @param distance distance to move ( in meters )
* @return Pose distance meters in front of input pose.
*
* ( used in both path_align and dist_align )
*/
50 geometry_msgs::msg::Pose2D getForwardPose( const geometry_msgs::msg::Pose2D & pose, double distance );
} // namespace dwb_critics
#endif // DWB_CRITICS__ALIGNMENT_UTIL_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CRITICS__BASE_OBSTACLE_HPP_
#define DWB_CRITICS__BASE_OBSTACLE_HPP_
#include <string>
#include <vector>
#include <utility>
#include "dwb_core/trajectory_critic.hpp"
namespace dwb_critics
{
/**
* @class BaseObstacleCritic
* @brief Uses costmap 2d to assign negative costs if a circular robot would collide at any point of the trajectory.
*
* This class can only be used to figure out if a circular robot is in collision. If the cell corresponding
* with any of the poses in the Trajectory is an obstacle, inscribed obstacle or unknown, it will return a
* negative cost. Otherwise it will return either the final pose's cost, or the sum of all poses, depending
* on the sum_scores parameter.
*
* Other classes ( like ObstacleFootprintCritic ) can do more advanced checking for collisions.
*/
57 class BaseObstacleCritic : public dwb_core::TrajectoryCritic
{
public:
void onInit( ) override;
double scoreTrajectory( const dwb_msgs::msg::Trajectory2D & traj ) override;
void addCriticVisualization(
std::vector<std::pair<std::string, std::vector<float>>> & cost_channels ) override;
/**
* @brief Return the obstacle score for a particular pose
* @param pose Pose to check
*/
virtual double scorePose( const geometry_msgs::msg::Pose2D & pose );
/**
* @brief Check to see whether a given cell cost is valid for driving through.
* @param cost Cost of the cell
* @return Return true if valid cell
*/
virtual bool isValidCost( const unsigned char cost );
protected:
nav2_costmap_2d::Costmap2D * costmap_;
bool sum_scores_;
};
} // namespace dwb_critics
#endif // DWB_CRITICS__BASE_OBSTACLE_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CRITICS__GOAL_ALIGN_HPP_
#define DWB_CRITICS__GOAL_ALIGN_HPP_
#include <vector>
#include <string>
#include "dwb_critics/goal_dist.hpp"
namespace dwb_critics
{
/**
* @class GoalAlignCritic
* @brief Scores trajectories based on whether the robot ends up pointing toward the eventual goal
*
* Similar to GoalDistCritic, this critic finds the pose from the global path farthest from the robot
* that is still on the costmap and then evaluates how far the front of the robot is from that point.
* This works as a proxy to calculating which way the robot should be pointing.
*/
52 class GoalAlignCritic : public GoalDistCritic
{
public:
55 GoalAlignCritic( )
: forward_point_distance_( 0.0 ) {}
void onInit( ) override;
bool prepare(
const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
const geometry_msgs::msg::Pose2D & goal, const nav_2d_msgs::msg::Path2D & global_plan ) override;
double scorePose( const geometry_msgs::msg::Pose2D & pose ) override;
protected:
double forward_point_distance_;
};
} // namespace dwb_critics
#endif // DWB_CRITICS__GOAL_ALIGN_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CRITICS__GOAL_DIST_HPP_
#define DWB_CRITICS__GOAL_DIST_HPP_
#include <vector>
#include "dwb_critics/map_grid.hpp"
namespace dwb_critics
{
/**
* @class GoalDistCritic
* @brief Scores trajectories based on how far along the global path they end up.
*
* This trajectory critic helps ensure progress along the global path. It finds the pose from the
* global path farthest from the robot that is still on the costmap, and aims for that point by
* assigning the lowest cost to the cell corresponding with that farthest pose.
*/
50 class GoalDistCritic : public MapGridCritic
{
public:
bool prepare(
const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
const geometry_msgs::msg::Pose2D & goal, const nav_2d_msgs::msg::Path2D & global_plan ) override;
protected:
bool getLastPoseOnCostmap(
const nav_2d_msgs::msg::Path2D & global_plan, unsigned int & x,
unsigned int & y );
};
} // namespace dwb_critics
#endif // DWB_CRITICS__GOAL_DIST_HPP_
1 /*
* Copyright ( c ) 2012, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES ( INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE )
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CRITICS__LINE_ITERATOR_HPP_
#define DWB_CRITICS__LINE_ITERATOR_HPP_
#include <stdlib.h>
namespace dwb_critics
{
/** An iterator implementing Bresenham Ray-Tracing. */
38 class LineIterator
{
public:
41 LineIterator( int x0, int y0, int x1, int y1 )
: x0_( x0 ),
y0_( y0 ),
x1_( x1 ),
y1_( y1 ),
x_( x0 ), // X and Y start of at first endpoint.
y_( y0 ),
deltax_( abs( x1 - x0 ) ),
deltay_( abs( y1 - y0 ) ),
curpixel_( 0 )
{
if ( x1_ >= x0_ ) { // The x-values are increasing
xinc1_ = 1;
xinc2_ = 1;
} else { // The x-values are decreasing
xinc1_ = -1;
xinc2_ = -1;
}
if ( y1_ >= y0_ ) { // The y-values are increasing
yinc1_ = 1;
yinc2_ = 1;
} else { // The y-values are decreasing
yinc1_ = -1;
yinc2_ = -1;
}
if ( deltax_ >= deltay_ ) { // There is at least one x-value for every y-value
xinc1_ = 0; // Don't change the x when numerator >= denominator
yinc2_ = 0; // Don't change the y for every iteration
den_ = deltax_;
num_ = deltax_ / 2;
numadd_ = deltay_;
numpixels_ = deltax_; // There are more x-values than y-values
} else { // There is at least one y-value for every x-value
xinc2_ = 0; // Don't change the x for every iteration
yinc1_ = 0; // Don't change the y when numerator >= denominator
den_ = deltay_;
num_ = deltay_ / 2;
numadd_ = deltax_;
numpixels_ = deltay_; // There are more y-values than x-values
}
}
85 bool isValid( ) const
{
return curpixel_ <= numpixels_;
}
90 void advance( )
{
num_ += numadd_; // Increase the numerator by the top of the fraction
if ( num_ >= den_ ) { // Check if numerator >= denominator
num_ -= den_; // Calculate the new numerator value
x_ += xinc1_; // Change the x as appropriate
y_ += yinc1_; // Change the y as appropriate
}
x_ += xinc2_; // Change the x as appropriate
y_ += yinc2_; // Change the y as appropriate
curpixel_++;
}
104 int getX( ) const
{
return x_;
}
108 int getY( ) const
{
return y_;
}
113 int getX0( ) const
{
return x0_;
}
117 int getY0( ) const
{
return y0_;
}
122 int getX1( ) const
{
return x1_;
}
126 int getY1( ) const
{
return y1_;
}
private:
int x0_; ///< X coordinate of first end point.
int y0_; ///< Y coordinate of first end point.
int x1_; ///< X coordinate of second end point.
int y1_; ///< Y coordinate of second end point.
int x_; ///< X coordinate of current point.
int y_; ///< Y coordinate of current point.
int deltax_; ///< Difference between Xs of endpoints.
int deltay_; ///< Difference between Ys of endpoints.
int curpixel_; ///< index of current point in line loop.
int xinc1_, xinc2_, yinc1_, yinc2_;
int den_, num_, numadd_, numpixels_;
};
} // end namespace dwb_critics
#endif // DWB_CRITICS__LINE_ITERATOR_HPP_
/*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CRITICS__MAP_GRID_HPP_
#define DWB_CRITICS__MAP_GRID_HPP_
#include <vector>
#include <memory>
#include <string>
#include <utility>
#include "dwb_core/trajectory_critic.hpp"
#include "costmap_queue/costmap_queue.hpp"
namespace dwb_critics
{
/**
* @class MapGridCritic
* @brief breadth-first scoring of all the cells in the costmap
*
* This TrajectoryCritic assigns a score to every cell in the costmap based on
* the distance to the cell from some set of source points. The cells corresponding
* with the source points are marked with some initial score, and then every other cell
* is updated with a score based on its relation to the closest source cell, based on a
* breadth-first exploration of the cells of the costmap.
*
* This approach was chosen for computational efficiency, such that each trajectory
* need not be compared to the list of source points.
*/
61 class MapGridCritic : public dwb_core::TrajectoryCritic
{
public:
// Standard TrajectoryCritic Interface
void onInit( ) override;
double scoreTrajectory( const dwb_msgs::msg::Trajectory2D & traj ) override;
void addCriticVisualization(
std::vector<std::pair<std::string, std::vector<float>>> & cost_channels ) override;
double getScale( ) const override {return costmap_->getResolution( ) * 0.5 * scale_;}
// Helper Functions
/**
* @brief Retrieve the score for a single pose
* @param pose The pose to score, assumed to be in the same frame as the costmap
* @return The score associated with the cell of the costmap where the pose lies
*/
virtual double scorePose( const geometry_msgs::msg::Pose2D & pose );
/**
* @brief Retrieve the score for a particular cell of the costmap
* @param x x-coordinate within the costmap
* @param y y-coordinate within the costmap
* @return the score associated with that cell.
*/
85 inline double getScore( unsigned int x, unsigned int y )
{
return cell_values_[costmap_->getIndex( x, y )];
}
/**
* @brief Sets the score of a particular cell to the obstacle cost
* @param index Index of the cell to mark
*/
94 void setAsObstacle( unsigned int index );
protected:
/**
* @brief Separate modes for aggregating scores across the multiple poses in a trajectory.
*
* Last returns the score associated with the last pose in the trajectory
* Sum returns the sum of all the scores
* Product returns the product of all the ( non-zero ) scores
*/
// cppcheck-suppress syntaxError
105 enum class ScoreAggregationType {Last, Sum, Product};
/**
* @class MapGridQueue
* @brief Subclass of CostmapQueue that avoids Obstacles and Unknown Values
*/
111 class MapGridQueue : public costmap_queue::CostmapQueue
{
public:
114 MapGridQueue( nav2_costmap_2d::Costmap2D & costmap, MapGridCritic & parent )
: costmap_queue::CostmapQueue( costmap, true ), parent_( parent ) {}
virtual ~MapGridQueue( ) = default;
bool validCellToQueue( const costmap_queue::CellData & cell ) override;
protected:
MapGridCritic & parent_;
};
/**
* @brief Clear the queuDWB_CRITICS_MAP_GRID_He and set cell_values_ to the appropriate number of unreachableCellScore
*/
126 void reset( ) override;
/**
* @brief Go through the queue and set the cells to the Manhattan distance from their parents
*/
131 void propogateManhattanDistances( );
std::shared_ptr<MapGridQueue> queue_;
nav2_costmap_2d::Costmap2D * costmap_;
std::vector<double> cell_values_;
double obstacle_score_, unreachable_score_; ///< Special cell_values
bool stop_on_failure_;
ScoreAggregationType aggregationType_;
};
} // namespace dwb_critics
#endif // DWB_CRITICS__MAP_GRID_HPP_
/*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CRITICS__OBSTACLE_FOOTPRINT_HPP_
#define DWB_CRITICS__OBSTACLE_FOOTPRINT_HPP_
#include <vector>
#include "dwb_critics/base_obstacle.hpp"
namespace dwb_critics
{
typedef std::vector<geometry_msgs::msg::Point> Footprint;
/**
* @brief Transform the footprint spec to be centered at the given pose
* @param pose Robot pose
* @param footprint_spec List of points that make up the footprint spec, centered at 0, 0
* @return oriented footprint
*/
51 Footprint getOrientedFootprint(
const geometry_msgs::msg::Pose2D & pose,
const Footprint & footprint_spec );
/**
* @class ObstacleFootprintCritic
* @brief Uses costmap 2d to assign negative costs if robot footprint is in obstacle on any point of the trajectory.
*
* Internally, this technically only checks if the border of the footprint collides with anything for computational
* efficiency. This is valid if the obstacles in the local costmap are inflated.
*
* A more robust class could check every cell within the robot's footprint without inflating the obstacles,
* at some computational cost. That is left as an excercise to the reader.
*/
65 class ObstacleFootprintCritic : public BaseObstacleCritic
{
public:
bool prepare(
const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
const geometry_msgs::msg::Pose2D & goal, const nav_2d_msgs::msg::Path2D & global_plan ) override;
double scorePose( const geometry_msgs::msg::Pose2D & pose ) override;
virtual double scorePose(
const geometry_msgs::msg::Pose2D & pose,
const Footprint & oriented_footprint );
double getScale( ) const override {return costmap_->getResolution( ) * scale_;}
protected:
/**
* @brief Rasterizes a line in the costmap grid and checks for collisions
* @param x0 The x position of the first cell in grid coordinates
* @param y0 The y position of the first cell in grid coordinates
* @param x1 The x position of the second cell in grid coordinates
* @param y1 The y position of the second cell in grid coordinates
* @return A positive cost for a legal line... negative otherwise
*/
double lineCost( int x0, int x1, int y0, int y1 );
/**
* @brief Checks the cost of a point in the costmap
* @param x The x position of the point in cell coordinates
* @param y The y position of the point in cell coordinates
* @return A positive cost for a legal point... negative otherwise
*/
94 double pointCost( int x, int y );
Footprint footprint_spec_;
};
} // namespace dwb_critics
#endif // DWB_CRITICS__OBSTACLE_FOOTPRINT_HPP_
/*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CRITICS__OSCILLATION_HPP_
#define DWB_CRITICS__OSCILLATION_HPP_
#include <vector>
#include <string>
#include <chrono>
#include "dwb_core/trajectory_critic.hpp"
using namespace std::chrono_literals; // NOLINT
namespace dwb_critics
{
/**
* @class OscillationCritic
* @brief Checks to see whether the sign of the commanded velocity flips frequently
*
* This critic figures out if the commanded trajectories are oscillating by seeing
* if one of the dimensions ( x, y, theta ) flips from positive to negative and then back
* ( or vice versa ) without moving sufficiently far or waiting sufficiently long.
*
* Scenario 1: Robot moves one meter forward, and then two millimeters backward.
* Another forward motion would be considered oscillating, since the x dimension would then
* flip from positive to negative and then back to negative. Hence, when scoring different
* trajectories, positive velocity commands will get the oscillation_score ( -5.0, or invalid )
* and only negative velocity commands will be considered valid.
* Scenario 2: Robot moves one meter forward, and then one meter backward.
* The robot has thus moved one meter since flipping the sign of the x direction, which
* is greater than our oscillation_reset_dist, so its not considered oscillating, so all
* trajectories are considered valid.
*
* Note: The critic will only check oscillations in the x dimension while it exceeds
* a particular value ( x_only_threshold_ ). If it dips below that magnitude, it will
* also check for oscillations in the y and theta dimensions. If x_only_threshold_ is
* negative, then the critic will always check all dimensions.
*
* Implementation Details:
* The critic saves the robot's current position when it prepares, and what the actual
* commanded velocity was during the debrief step. Upon debriefing, if the sign of any of
* dimensions has flipped since the last command, the position is saved as prev_stationary_pose_.
*
* If the linear or angular distance from prev_stationary_pose_ to the current pose exceeds
* the limits, the oscillation flags are reset so the previous sign change is no longer remembered.
* This assumes that oscillation_reset_dist_ or oscillation_reset_angle_ are positive. Otherwise,
* it uses a time based delay reset function.
*/
82 class OscillationCritic : public dwb_core::TrajectoryCritic
{
public:
OscillationCritic( )
: oscillation_reset_time_( 0s ) {}
void onInit( ) override;
88 bool prepare(
const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
const geometry_msgs::msg::Pose2D & goal, const nav_2d_msgs::msg::Path2D & global_plan ) override;
91 double scoreTrajectory( const dwb_msgs::msg::Trajectory2D & traj ) override;
92 void reset( ) override;
93 void debrief( const nav_2d_msgs::msg::Twist2D & cmd_vel ) override;
private:
/**
* @class CommandTrend
* @brief Helper class for performing the same logic on the x, y and theta dimensions
*/
100 class CommandTrend
{
public:
103 CommandTrend( );
104 void reset( );
/**
* @brief update internal flags based on the commanded velocity
* @param velocity commanded velocity for the dimension this trend is tracking
* @return true if the sign has flipped
*/
111 bool update( double velocity );
/**
* @brief Check to see whether the proposed velocity would be considered oscillating
* @param velocity the velocity to evaluate
* @return true if the sign has flipped more than once
*/
118 bool isOscillating( double velocity );
/**
* @brief Check whether we are currently tracking a flipped sign
* @return True if the sign has flipped
*/
124 bool hasSignFlipped( );
private:
// Simple Enum for Tracking
// cppcheck-suppress syntaxError
enum class Sign { ZERO, POSITIVE, NEGATIVE };
Sign sign_;
bool positive_only_, negative_only_;
};
/**
* @brief Given a command that has been selected, track each component's sign for oscillations
* @param cmd_vel The command velocity selected by the algorithm
* @return True if the sign on any of the components flipped
*/
bool setOscillationFlags( const nav_2d_msgs::msg::Twist2D & cmd_vel );
/**
* @brief Return true if the robot has travelled far enough or waited long enough
*/
bool resetAvailable( );
CommandTrend x_trend_, y_trend_, theta_trend_;
double oscillation_reset_dist_, oscillation_reset_angle_, x_only_threshold_;
rclcpp::Duration oscillation_reset_time_;
// Cached square parameter
double oscillation_reset_dist_sq_;
// Saved positions
geometry_msgs::msg::Pose2D pose_, prev_stationary_pose_;
// Saved timestamp
rclcpp::Time prev_reset_time_;
rclcpp::Clock::SharedPtr clock_;
};
} // namespace dwb_critics
#endif // DWB_CRITICS__OSCILLATION_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CRITICS__PATH_ALIGN_HPP_
#define DWB_CRITICS__PATH_ALIGN_HPP_
#include <vector>
#include <string>
#include "dwb_critics/path_dist.hpp"
namespace dwb_critics
{
/**
* @class PathAlignCritic
* @brief Scores trajectories based on how far from the global path the front of the robot ends up.
*
* This uses the costmap grid as a proxy for calculating which way the robot should be facing relative
* to the global path. Instead of scoring how far the center of the robot is away from the global path,
* this critic calculates how far a point forward_point_distance in front of the robot is from the global
* path. This biases the planner toward trajectories that line up with the global plan.
*
* When the robot is near the end of the path, the scale of this critic is set to zero. When the projected
* point is past the global goal, we no longer want this critic to try to align to a part of the global path
* that isn't there.
*/
56 class PathAlignCritic : public PathDistCritic
{
public:
59 PathAlignCritic( )
: zero_scale_( false ), forward_point_distance_( 0.0 ) {}
void onInit( ) override;
bool prepare(
const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
const geometry_msgs::msg::Pose2D & goal, const nav_2d_msgs::msg::Path2D & global_plan ) override;
double getScale( ) const override;
double scorePose( const geometry_msgs::msg::Pose2D & pose ) override;
protected:
bool zero_scale_;
double forward_point_distance_;
};
} // namespace dwb_critics
#endif // DWB_CRITICS__PATH_ALIGN_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CRITICS__PATH_DIST_HPP_
#define DWB_CRITICS__PATH_DIST_HPP_
#include "dwb_critics/map_grid.hpp"
namespace dwb_critics
{
/**
* @class PathDistCritic
* @brief Scores trajectories based on how far from the global path they end up.
*/
45 class PathDistCritic : public MapGridCritic
{
public:
bool prepare(
const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
const geometry_msgs::msg::Pose2D & goal, const nav_2d_msgs::msg::Path2D & global_plan ) override;
};
} // namespace dwb_critics
#endif // DWB_CRITICS__PATH_DIST_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CRITICS__PREFER_FORWARD_HPP_
#define DWB_CRITICS__PREFER_FORWARD_HPP_
#include <string>
#include "dwb_core/trajectory_critic.hpp"
namespace dwb_critics
{
/**
* @class PreferForwardCritic
* @brief Penalize trajectories with move backwards and/or turn too much
*
* Has three different scoring conditions:
* 1 ) If the trajectory's x velocity is negative, return the penalty
* 2 ) If the trajectory's x is low and the theta is also low, return the penalty.
* 3 ) Otherwise, return a scaled version of the trajectory's theta.
*/
53 class PreferForwardCritic : public dwb_core::TrajectoryCritic
{
public:
56 PreferForwardCritic( )
: penalty_( 1.0 ), strafe_x_( 0.1 ), strafe_theta_( 0.2 ), theta_scale_( 10.0 ) {}
void onInit( ) override;
double scoreTrajectory( const dwb_msgs::msg::Trajectory2D & traj ) override;
private:
double penalty_, strafe_x_, strafe_theta_, theta_scale_;
};
} // namespace dwb_critics
#endif // DWB_CRITICS__PREFER_FORWARD_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CRITICS__ROTATE_TO_GOAL_HPP_
#define DWB_CRITICS__ROTATE_TO_GOAL_HPP_
#include <string>
#include <vector>
#include "dwb_core/trajectory_critic.hpp"
namespace dwb_critics
{
/**
* @class RotateToGoalCritic
* @brief Forces the commanded trajectories to only be rotations if within a certain distance window
*
* This used to be built in to the DWA Local Planner as the LatchedStopRotate controller,
* but has been moved to a critic for consistency.
*
* The critic has three distinct phases.
* 1 ) If the current pose is outside xy_goal_tolerance LINEAR distance from the goal pose, this critic
* will just return score 0.0.
* 2 ) If within the xy_goal_tolerance and the robot is still moving with non-zero linear motion, this critic
* will only allow trajectories that are slower than the current speed in order to stop the robot ( within
* the robot's acceleration limits ). The returned score will be the robot's linear speed squared, multiplied
* by the slowing_factor parameter ( default 5.0 ) added to the result of scoreRotation.
* 3 ) If within the xy_goal_tolerance and the robot has sufficiently small linear motion, this critic will
* score trajectories that have linear movement as invalid and score the rest based on the result of the
* scoreRotation method
*
* The scoreRotation method can be overriden, but the default behavior is to return the shortest angular distance
* between the goal pose and a pose from the trajectory. Which pose depends on the lookahead_time parameter.
* * If the lookahead_time parameter is negative, the pose evaluated will be the last pose in the trajectory,
* which is the same as DWA's behavior. This is the default.
* * Otherwise, a new pose will be projected using the dwb_local_planner::projectPose. By using a lookahead
* time shorter than sim_time, the critic will be less concerned about overshooting the goal yaw and thus will
* continue to turn faster for longer.
*/
70 class RotateToGoalCritic : public dwb_core::TrajectoryCritic
{
public:
void onInit( ) override;
void reset( ) override;
bool prepare(
const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
const geometry_msgs::msg::Pose2D & goal, const nav_2d_msgs::msg::Path2D & global_plan ) override;
double scoreTrajectory( const dwb_msgs::msg::Trajectory2D & traj ) override;
/**
* @brief Assuming that this is an actual rotation when near the goal, score the trajectory.
*
* This ( easily overridden ) method assumes that the critic is in the third phase ( as described above )
* and returns a numeric score for the trajectory relative to the goal yaw.
* @param traj Trajectory to score
* @return numeric score
*/
virtual double scoreRotation( const dwb_msgs::msg::Trajectory2D & traj );
private:
bool in_window_;
bool rotating_;
double goal_yaw_;
double xy_goal_tolerance_;
double xy_goal_tolerance_sq_; ///< Cached squared tolerance
double current_xy_speed_sq_, stopped_xy_velocity_sq_;
double slowing_factor_;
double lookahead_time_;
};
} // namespace dwb_critics
#endif // DWB_CRITICS__ROTATE_TO_GOAL_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_CRITICS__TWIRLING_HPP_
#define DWB_CRITICS__TWIRLING_HPP_
#include "dwb_core/trajectory_critic.hpp"
namespace dwb_critics
{
/**
* @class TwirlingCritic
* @brief Penalize trajectories with rotational velocities
*
* This class provides a cost based on how much a robot "twirls" on its way to the goal. With
* differential-drive robots, there isn't a choice, but with holonomic or near-holonomic robots,
* sometimes a robot spins more than you'd like on its way to a goal. This class provides a way
* to assign a penalty purely to rotational velocities.
*/
51 class TwirlingCritic : public dwb_core::TrajectoryCritic
{
public:
void onInit( ) override;
double scoreTrajectory( const dwb_msgs::msg::Trajectory2D & traj ) override;
};
} // namespace dwb_critics
#endif // DWB_CRITICS__TWIRLING_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "dwb_critics/alignment_util.hpp"
#include <cmath>
using std::cos;
using std::sin;
namespace dwb_critics
{
43 geometry_msgs::msg::Pose2D getForwardPose( const geometry_msgs::msg::Pose2D & pose, double distance )
{
geometry_msgs::msg::Pose2D forward_pose;
forward_pose.x = pose.x + distance * cos( pose.theta );
forward_pose.y = pose.y + distance * sin( pose.theta );
forward_pose.theta = pose.theta;
return forward_pose;
}
} // namespace dwb_critics
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <vector>
#include <string>
#include <utility>
#include "dwb_critics/base_obstacle.hpp"
#include "dwb_core/exceptions.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "nav2_costmap_2d/cost_values.hpp"
#include "nav2_util/node_utils.hpp"
44 PLUGINLIB_EXPORT_CLASS( dwb_critics::BaseObstacleCritic, dwb_core::TrajectoryCritic )
namespace dwb_critics
{
void BaseObstacleCritic::onInit( )
{
costmap_ = costmap_ros_->getCostmap( );
auto node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
nav2_util::declare_parameter_if_not_declared(
node,
dwb_plugin_name_ + "." + name_ + ".sum_scores", rclcpp::ParameterValue( false ) );
node->get_parameter( dwb_plugin_name_ + "." + name_ + ".sum_scores", sum_scores_ );
}
double BaseObstacleCritic::scoreTrajectory( const dwb_msgs::msg::Trajectory2D & traj )
{
double score = 0.0;
for ( unsigned int i = 0; i < traj.poses.size( ); ++i ) {
double pose_score = scorePose( traj.poses[i] );
// Optimized/branchless version of if ( sum_scores_ ) score += pose_score,
// else score = pose_score;
score = static_cast<double>( sum_scores_ ) * score + pose_score;
}
return score;
}
double BaseObstacleCritic::scorePose( const geometry_msgs::msg::Pose2D & pose )
{
unsigned int cell_x, cell_y;
if ( !costmap_->worldToMap( pose.x, pose.y, cell_x, cell_y ) ) {
throw dwb_core::
IllegalTrajectoryException( name_, "Trajectory Goes Off Grid." );
}
unsigned char cost = costmap_->getCost( cell_x, cell_y );
if ( !isValidCost( cost ) ) {
throw dwb_core::
IllegalTrajectoryException( name_, "Trajectory Hits Obstacle." );
}
return cost;
}
bool BaseObstacleCritic::isValidCost( const unsigned char cost )
{
return cost != nav2_costmap_2d::LETHAL_OBSTACLE &&
cost != nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE &&
cost != nav2_costmap_2d::NO_INFORMATION;
}
void BaseObstacleCritic::addCriticVisualization(
std::vector<std::pair<std::string, std::vector<float>>> & cost_channels )
{
std::pair<std::string, std::vector<float>> grid_scores;
grid_scores.first = name_;
unsigned int size_x = costmap_->getSizeInCellsX( );
unsigned int size_y = costmap_->getSizeInCellsY( );
grid_scores.second.resize( size_x * size_y );
unsigned int i = 0;
for ( unsigned int cy = 0; cy < size_y; cy++ ) {
for ( unsigned int cx = 0; cx < size_x; cx++ ) {
grid_scores.second[i] = costmap_->getCost( cx, cy );
i++;
}
}
cost_channels.push_back( grid_scores );
}
} // namespace dwb_critics
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "dwb_critics/goal_align.hpp"
#include <vector>
#include <string>
#include "dwb_critics/alignment_util.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "nav_2d_utils/parameters.hpp"
namespace dwb_critics
{
45 void GoalAlignCritic::onInit( )
{
GoalDistCritic::onInit( );
stop_on_failure_ = false;
auto node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
forward_point_distance_ = nav_2d_utils::searchAndGetParam(
node,
dwb_plugin_name_ + "." + name_ + ".forward_point_distance", 0.325 );
}
60 bool GoalAlignCritic::prepare(
61 const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
62 const geometry_msgs::msg::Pose2D & goal,
63 const nav_2d_msgs::msg::Path2D & global_plan )
{
// we want the robot nose to be drawn to its final position
// ( before robot turns towards goal orientation ), not the end of the
// path for the robot center. Choosing the final position after
// turning towards goal orientation causes instability when the
// robot needs to make a 180 degree turn at the end
double angle_to_goal = atan2( goal.y - pose.y, goal.x - pose.x );
nav_2d_msgs::msg::Path2D target_poses = global_plan;
target_poses.poses.back( ).x += forward_point_distance_ * cos( angle_to_goal );
target_poses.poses.back( ).y += forward_point_distance_ * sin( angle_to_goal );
return GoalDistCritic::prepare( pose, vel, goal, target_poses );
}
79 double GoalAlignCritic::scorePose( const geometry_msgs::msg::Pose2D & pose )
{
return GoalDistCritic::scorePose( getForwardPose( pose, forward_point_distance_ ) );
}
} // namespace dwb_critics
86 PLUGINLIB_EXPORT_CLASS( dwb_critics::GoalAlignCritic, dwb_core::TrajectoryCritic )
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "dwb_critics/goal_dist.hpp"
#include <vector>
#include "pluginlib/class_list_macros.hpp"
#include "nav_2d_utils/path_ops.hpp"
#include "nav2_costmap_2d/cost_values.hpp"
namespace dwb_critics
{
43 bool GoalDistCritic::prepare(
44 const geometry_msgs::msg::Pose2D &, const nav_2d_msgs::msg::Twist2D &,
45 const geometry_msgs::msg::Pose2D &,
46 const nav_2d_msgs::msg::Path2D & global_plan )
{
reset( );
unsigned int local_goal_x, local_goal_y;
if ( !getLastPoseOnCostmap( global_plan, local_goal_x, local_goal_y ) ) {
return false;
}
// Enqueue just the last pose
int index = costmap_->getIndex( local_goal_x, local_goal_y );
cell_values_[index] = 0.0;
queue_->enqueueCell( local_goal_x, local_goal_y );
propogateManhattanDistances( );
return true;
}
65 bool GoalDistCritic::getLastPoseOnCostmap(
66 const nav_2d_msgs::msg::Path2D & global_plan,
unsigned int & x, unsigned int & y )
{
nav_2d_msgs::msg::Path2D adjusted_global_plan = nav_2d_utils::adjustPlanResolution(
global_plan,
costmap_->getResolution( ) );
bool started_path = false;
// skip global path points until we reach the border of the local map
for ( unsigned int i = 0; i < adjusted_global_plan.poses.size( ); ++i ) {
double g_x = adjusted_global_plan.poses[i].x;
double g_y = adjusted_global_plan.poses[i].y;
unsigned int map_x, map_y;
if ( costmap_->worldToMap(
g_x, g_y, map_x,
map_y ) && costmap_->getCost( map_x, map_y ) != nav2_costmap_2d::NO_INFORMATION )
{
// Still on the costmap. Continue.
x = map_x;
y = map_y;
started_path = true;
} else if ( started_path ) {
// Off the costmap after being on the costmap. Return the last saved indices.
return true;
}
// else, we have not yet found a point on the costmap, so we just continue
}
if ( started_path ) {
return true;
} else {
RCLCPP_ERROR(
rclcpp::get_logger(
"GoalDistCritic" ), "None of the points of the global plan were in the local costmap." );
return false;
}
}
} // namespace dwb_critics
106 PLUGINLIB_EXPORT_CLASS( dwb_critics::GoalDistCritic, dwb_core::TrajectoryCritic )
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "dwb_critics/map_grid.hpp"
#include <cmath>
#include <string>
#include <vector>
#include <utility>
#include <algorithm>
#include <memory>
#include "dwb_core/exceptions.hpp"
#include "nav2_costmap_2d/cost_values.hpp"
#include "nav2_util/node_utils.hpp"
using std::abs;
using costmap_queue::CellData;
namespace dwb_critics
{
// Customization of the CostmapQueue validCellToQueue method
53 bool MapGridCritic::MapGridQueue::validCellToQueue( const costmap_queue::CellData & /*cell*/ )
{
return true;
}
58 void MapGridCritic::onInit( )
{
costmap_ = costmap_ros_->getCostmap( );
queue_ = std::make_shared<MapGridQueue>( *costmap_, *this );
// Always set to true, but can be overriden by subclasses
stop_on_failure_ = true;
auto node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
nav2_util::declare_parameter_if_not_declared(
node,
dwb_plugin_name_ + "." + name_ + ".aggregation_type",
rclcpp::ParameterValue( std::string( "last" ) ) );
std::string aggro_str;
node->get_parameter( dwb_plugin_name_ + "." + name_ + ".aggregation_type", aggro_str );
std::transform( aggro_str.begin( ), aggro_str.end( ), aggro_str.begin( ), ::tolower );
if ( aggro_str == "last" ) {
aggregationType_ = ScoreAggregationType::Last;
} else if ( aggro_str == "sum" ) {
aggregationType_ = ScoreAggregationType::Sum;
} else if ( aggro_str == "product" ) {
aggregationType_ = ScoreAggregationType::Product;
} else {
RCLCPP_ERROR(
rclcpp::get_logger(
"MapGridCritic" ), "aggregation_type parameter \"%s\" invalid. Using Last.",
aggro_str.c_str( ) );
aggregationType_ = ScoreAggregationType::Last;
}
}
94 void MapGridCritic::setAsObstacle( unsigned int index )
{
cell_values_[index] = obstacle_score_;
}
99 void MapGridCritic::reset( )
{
queue_->reset( );
cell_values_.resize( costmap_->getSizeInCellsX( ) * costmap_->getSizeInCellsY( ) );
obstacle_score_ = static_cast<double>( cell_values_.size( ) );
unreachable_score_ = obstacle_score_ + 1.0;
std::fill( cell_values_.begin( ), cell_values_.end( ), unreachable_score_ );
}
108 void MapGridCritic::propogateManhattanDistances( )
{
while ( !queue_->isEmpty( ) ) {
costmap_queue::CellData cell = queue_->getNextCell( );
cell_values_[cell.index_] = CellData::absolute_difference( cell.src_x_, cell.x_ ) +
CellData::absolute_difference( cell.src_y_, cell.y_ );
}
}
117 double MapGridCritic::scoreTrajectory( const dwb_msgs::msg::Trajectory2D & traj )
{
double score = 0.0;
unsigned int start_index = 0;
if ( aggregationType_ == ScoreAggregationType::Product ) {
score = 1.0;
} else if ( aggregationType_ == ScoreAggregationType::Last && !stop_on_failure_ ) {
start_index = traj.poses.size( ) - 1;
}
double grid_dist;
for ( unsigned int i = start_index; i < traj.poses.size( ); ++i ) {
grid_dist = scorePose( traj.poses[i] );
if ( stop_on_failure_ ) {
if ( grid_dist == obstacle_score_ ) {
throw dwb_core::
IllegalTrajectoryException( name_, "Trajectory Hits Obstacle." );
} else if ( grid_dist == unreachable_score_ ) {
throw dwb_core::
IllegalTrajectoryException( name_, "Trajectory Hits Unreachable Area." );
}
}
switch ( aggregationType_ ) {
case ScoreAggregationType::Last:
score = grid_dist;
break;
case ScoreAggregationType::Sum:
score += grid_dist;
break;
case ScoreAggregationType::Product:
if ( score > 0 ) {
score *= grid_dist;
}
break;
}
}
return score;
}
158 double MapGridCritic::scorePose( const geometry_msgs::msg::Pose2D & pose )
{
unsigned int cell_x, cell_y;
// we won't allow trajectories that go off the map... shouldn't happen that often anyways
if ( !costmap_->worldToMap( pose.x, pose.y, cell_x, cell_y ) ) {
throw dwb_core::
IllegalTrajectoryException( name_, "Trajectory Goes Off Grid." );
}
return getScore( cell_x, cell_y );
}
169 void MapGridCritic::addCriticVisualization(
std::vector<std::pair<std::string, std::vector<float>>> & cost_channels )
{
std::pair<std::string, std::vector<float>> grid_scores;
grid_scores.first = name_;
nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap( );
unsigned int size_x = costmap->getSizeInCellsX( );
unsigned int size_y = costmap->getSizeInCellsY( );
grid_scores.second.resize( size_x * size_y );
unsigned int i = 0;
for ( unsigned int cy = 0; cy < size_y; cy++ ) {
for ( unsigned int cx = 0; cx < size_x; cx++ ) {
grid_scores.second[i] = getScore( cx, cy );
i++;
}
}
cost_channels.push_back( grid_scores );
}
} // namespace dwb_critics
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "dwb_critics/obstacle_footprint.hpp"
#include <algorithm>
#include <vector>
#include "dwb_critics/line_iterator.hpp"
#include "dwb_core/exceptions.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "nav2_costmap_2d/cost_values.hpp"
43 PLUGINLIB_EXPORT_CLASS( dwb_critics::ObstacleFootprintCritic, dwb_core::TrajectoryCritic )
namespace dwb_critics
{
Footprint getOrientedFootprint(
const geometry_msgs::msg::Pose2D & pose,
const Footprint & footprint_spec )
{
std::vector<geometry_msgs::msg::Point> oriented_footprint;
oriented_footprint.resize( footprint_spec.size( ) );
double cos_th = cos( pose.theta );
double sin_th = sin( pose.theta );
for ( unsigned int i = 0; i < footprint_spec.size( ); ++i ) {
geometry_msgs::msg::Point & new_pt = oriented_footprint[i];
new_pt.x = pose.x + footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th;
new_pt.y = pose.y + footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th;
}
return oriented_footprint;
}
bool ObstacleFootprintCritic::prepare(
const geometry_msgs::msg::Pose2D &, const nav_2d_msgs::msg::Twist2D &,
const geometry_msgs::msg::Pose2D &, const nav_2d_msgs::msg::Path2D & )
{
footprint_spec_ = costmap_ros_->getRobotFootprint( );
if ( footprint_spec_.size( ) == 0 ) {
RCLCPP_ERROR(
rclcpp::get_logger( "ObstacleFootprintCritic" ),
"Footprint spec is empty, maybe missing call to setFootprint?" );
return false;
}
return true;
}
double ObstacleFootprintCritic::scorePose( const geometry_msgs::msg::Pose2D & pose )
{
unsigned int cell_x, cell_y;
if ( !costmap_->worldToMap( pose.x, pose.y, cell_x, cell_y ) ) {
throw dwb_core::
IllegalTrajectoryException( name_, "Trajectory Goes Off Grid." );
}
return scorePose( pose, getOrientedFootprint( pose, footprint_spec_ ) );
}
double ObstacleFootprintCritic::scorePose(
const geometry_msgs::msg::Pose2D &,
const Footprint & footprint )
{
// now we really have to lay down the footprint in the costmap grid
unsigned int x0, x1, y0, y1;
double line_cost = 0.0;
double footprint_cost = 0.0;
// we need to rasterize each line in the footprint
for ( unsigned int i = 0; i < footprint.size( ) - 1; ++i ) {
// get the cell coord of the first point
if ( !costmap_->worldToMap( footprint[i].x, footprint[i].y, x0, y0 ) ) {
throw dwb_core::
IllegalTrajectoryException( name_, "Footprint Goes Off Grid." );
}
// get the cell coord of the second point
if ( !costmap_->worldToMap( footprint[i + 1].x, footprint[i + 1].y, x1, y1 ) ) {
throw dwb_core::
IllegalTrajectoryException( name_, "Footprint Goes Off Grid." );
}
line_cost = lineCost( x0, x1, y0, y1 );
footprint_cost = std::max( line_cost, footprint_cost );
}
// we also need to connect the first point in the footprint to the last point
// get the cell coord of the last point
if ( !costmap_->worldToMap( footprint.back( ).x, footprint.back( ).y, x0, y0 ) ) {
throw dwb_core::
IllegalTrajectoryException( name_, "Footprint Goes Off Grid." );
}
// get the cell coord of the first point
if ( !costmap_->worldToMap( footprint.front( ).x, footprint.front( ).y, x1, y1 ) ) {
throw dwb_core::
IllegalTrajectoryException( name_, "Footprint Goes Off Grid." );
}
line_cost = lineCost( x0, x1, y0, y1 );
footprint_cost = std::max( line_cost, footprint_cost );
// if all line costs are legal... then we can return that the footprint is legal
return footprint_cost;
}
double ObstacleFootprintCritic::lineCost( int x0, int x1, int y0, int y1 )
{
double line_cost = 0.0;
double point_cost = -1.0;
for ( LineIterator line( x0, y0, x1, y1 ); line.isValid( ); line.advance( ) ) {
point_cost = pointCost( line.getX( ), line.getY( ) ); // Score the current point
if ( line_cost < point_cost ) {
line_cost = point_cost;
}
}
return line_cost;
}
double ObstacleFootprintCritic::pointCost( int x, int y )
{
unsigned char cost = costmap_->getCost( x, y );
// if the cell is in an obstacle the path is invalid or unknown
if ( cost == nav2_costmap_2d::LETHAL_OBSTACLE ) {
throw dwb_core::
IllegalTrajectoryException( name_, "Trajectory Hits Obstacle." );
} else if ( cost == nav2_costmap_2d::NO_INFORMATION ) {
throw dwb_core::
IllegalTrajectoryException( name_, "Trajectory Hits Unknown Region." );
}
return cost;
}
} // namespace dwb_critics
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "dwb_critics/oscillation.hpp"
#include <chrono>
#include <cmath>
#include <string>
#include <vector>
#include "nav_2d_utils/parameters.hpp"
#include "nav2_util/node_utils.hpp"
#include "dwb_core/exceptions.hpp"
#include "pluginlib/class_list_macros.hpp"
45 PLUGINLIB_EXPORT_CLASS( dwb_critics::OscillationCritic, dwb_core::TrajectoryCritic )
namespace dwb_critics
{
OscillationCritic::CommandTrend::CommandTrend( )
{
reset( );
}
void OscillationCritic::CommandTrend::reset( )
{
sign_ = Sign::ZERO;
positive_only_ = false;
negative_only_ = false;
}
bool OscillationCritic::CommandTrend::update( double velocity )
{
bool flag_set = false;
if ( velocity < 0.0 ) {
if ( sign_ == Sign::POSITIVE ) {
negative_only_ = true;
flag_set = true;
}
sign_ = Sign::NEGATIVE;
} else if ( velocity > 0.0 ) {
if ( sign_ == Sign::NEGATIVE ) {
positive_only_ = true;
flag_set = true;
}
sign_ = Sign::POSITIVE;
}
return flag_set;
}
bool OscillationCritic::CommandTrend::isOscillating( double velocity )
{
return ( positive_only_ && velocity < 0.0 ) || ( negative_only_ && velocity > 0.0 );
}
bool OscillationCritic::CommandTrend::hasSignFlipped( )
{
return positive_only_ || negative_only_;
}
void OscillationCritic::onInit( )
{
auto node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
clock_ = node->get_clock( );
oscillation_reset_dist_ = nav_2d_utils::searchAndGetParam(
node,
dwb_plugin_name_ + "." + name_ + ".oscillation_reset_dist", 0.05 );
oscillation_reset_dist_sq_ = oscillation_reset_dist_ * oscillation_reset_dist_;
oscillation_reset_angle_ = nav_2d_utils::searchAndGetParam(
node,
dwb_plugin_name_ + "." + name_ + ".oscillation_reset_angle", 0.2 );
oscillation_reset_time_ = rclcpp::Duration::from_seconds(
nav_2d_utils::searchAndGetParam(
node,
dwb_plugin_name_ + "." + name_ + ".oscillation_reset_time", -1.0 ) );
nav2_util::declare_parameter_if_not_declared(
node,
dwb_plugin_name_ + "." + name_ + ".x_only_threshold", rclcpp::ParameterValue( 0.05 ) );
/**
* Historical Parameter Loading
* If x_only_threshold is set, use that.
* If min_speed_xy is set in the namespace ( as it is often used for trajectory generation ), use that.
* If min_trans_vel is set in the namespace, as it used to be used for trajectory generation, complain then use that.
* Otherwise, set x_only_threshold_ to 0.05
*/
node->get_parameter( dwb_plugin_name_ + "." + name_ + ".x_only_threshold", x_only_threshold_ );
// TODO( crdelsey ): How to handle searchParam?
// std::string resolved_name;
// if ( node->hasParam( "x_only_threshold" ) )
// {
// node->param( "x_only_threshold", x_only_threshold_ );
// }
// else if ( node->searchParam( "min_speed_xy", resolved_name ) )
// {
// node->param( resolved_name, x_only_threshold_ );
// }
// else if ( node->searchParam( "min_trans_vel", resolved_name ) )
// {
// ROS_WARN_NAMED( "OscillationCritic",
// "Parameter min_trans_vel is deprecated. "
// "Please use the name min_speed_xy or x_only_threshold instead." );
// node->param( resolved_name, x_only_threshold_ );
// }
// else
// {
// x_only_threshold_ = 0.05;
// }
reset( );
}
bool OscillationCritic::prepare(
const geometry_msgs::msg::Pose2D & pose,
const nav_2d_msgs::msg::Twist2D &,
const geometry_msgs::msg::Pose2D &,
const nav_2d_msgs::msg::Path2D & )
{
pose_ = pose;
return true;
}
void OscillationCritic::debrief( const nav_2d_msgs::msg::Twist2D & cmd_vel )
{
if ( setOscillationFlags( cmd_vel ) ) {
prev_stationary_pose_ = pose_;
prev_reset_time_ = clock_->now( );
}
// if we've got restrictions... check if we can reset any oscillation flags
if ( x_trend_.hasSignFlipped( ) || y_trend_.hasSignFlipped( ) || theta_trend_.hasSignFlipped( ) ) {
// Reset flags if enough time or distance has passed
if ( resetAvailable( ) ) {
reset( );
}
}
}
bool OscillationCritic::resetAvailable( )
{
if ( oscillation_reset_dist_ >= 0.0 ) {
double x_diff = pose_.x - prev_stationary_pose_.x;
double y_diff = pose_.y - prev_stationary_pose_.y;
double sq_dist = x_diff * x_diff + y_diff * y_diff;
if ( sq_dist > oscillation_reset_dist_sq_ ) {
return true;
}
}
if ( oscillation_reset_angle_ >= 0.0 ) {
double th_diff = pose_.theta - prev_stationary_pose_.theta;
if ( fabs( th_diff ) > oscillation_reset_angle_ ) {
return true;
}
}
if ( oscillation_reset_time_ >= rclcpp::Duration::from_seconds( 0.0 ) ) {
auto t_diff = ( clock_->now( ) - prev_reset_time_ );
if ( t_diff > oscillation_reset_time_ ) {
return true;
}
}
return false;
}
void OscillationCritic::reset( )
{
x_trend_.reset( );
y_trend_.reset( );
theta_trend_.reset( );
}
bool OscillationCritic::setOscillationFlags( const nav_2d_msgs::msg::Twist2D & cmd_vel )
{
bool flag_set = false;
// set oscillation flags for moving forward and backward
flag_set |= x_trend_.update( cmd_vel.x );
// we'll only set flags for strafing and rotating when we're not moving forward at all
if ( x_only_threshold_ < 0.0 || fabs( cmd_vel.x ) <= x_only_threshold_ ) {
flag_set |= y_trend_.update( cmd_vel.y );
flag_set |= theta_trend_.update( cmd_vel.theta );
}
return flag_set;
}
double OscillationCritic::scoreTrajectory( const dwb_msgs::msg::Trajectory2D & traj )
{
if ( x_trend_.isOscillating( traj.velocity.x ) ||
y_trend_.isOscillating( traj.velocity.y ) ||
theta_trend_.isOscillating( traj.velocity.theta ) )
{
throw dwb_core::
IllegalTrajectoryException( name_, "Trajectory is oscillating." );
}
return 0.0;
}
} // namespace dwb_critics
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "dwb_critics/path_align.hpp"
#include <vector>
#include <string>
#include "dwb_critics/alignment_util.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "nav_2d_utils/parameters.hpp"
namespace dwb_critics
{
45 void PathAlignCritic::onInit( )
{
PathDistCritic::onInit( );
stop_on_failure_ = false;
auto node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
forward_point_distance_ = nav_2d_utils::searchAndGetParam(
node,
dwb_plugin_name_ + "." + name_ + ".forward_point_distance", 0.325 );
}
60 bool PathAlignCritic::prepare(
61 const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
62 const geometry_msgs::msg::Pose2D & goal,
63 const nav_2d_msgs::msg::Path2D & global_plan )
{
double dx = pose.x - goal.x;
double dy = pose.y - goal.y;
double sq_dist = dx * dx + dy * dy;
if ( sq_dist > forward_point_distance_ * forward_point_distance_ ) {
zero_scale_ = false;
} else {
// once we are close to goal, trying to keep the nose close to anything destabilizes behavior.
zero_scale_ = true;
return true;
}
return PathDistCritic::prepare( pose, vel, goal, global_plan );
}
79 double PathAlignCritic::getScale( ) const
{
if ( zero_scale_ ) {
return 0.0;
} else {
return costmap_->getResolution( ) * 0.5 * scale_;
}
}
88 double PathAlignCritic::scorePose( const geometry_msgs::msg::Pose2D & pose )
{
return PathDistCritic::scorePose( getForwardPose( pose, forward_point_distance_ ) );
}
} // namespace dwb_critics
95 PLUGINLIB_EXPORT_CLASS( dwb_critics::PathAlignCritic, dwb_core::TrajectoryCritic )
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "dwb_critics/path_dist.hpp"
#include <vector>
#include "pluginlib/class_list_macros.hpp"
#include "nav_2d_utils/path_ops.hpp"
#include "nav2_costmap_2d/cost_values.hpp"
namespace dwb_critics
{
43 bool PathDistCritic::prepare(
44 const geometry_msgs::msg::Pose2D &, const nav_2d_msgs::msg::Twist2D &,
45 const geometry_msgs::msg::Pose2D &,
46 const nav_2d_msgs::msg::Path2D & global_plan )
{
reset( );
bool started_path = false;
nav_2d_msgs::msg::Path2D adjusted_global_plan =
nav_2d_utils::adjustPlanResolution( global_plan, costmap_->getResolution( ) );
if ( adjusted_global_plan.poses.size( ) != global_plan.poses.size( ) ) {
RCLCPP_DEBUG(
rclcpp::get_logger(
"PathDistCritic" ), "Adjusted global plan resolution, added %zu points",
adjusted_global_plan.poses.size( ) - global_plan.poses.size( ) );
}
unsigned int i;
// put global path points into local map until we reach the border of the local map
for ( i = 0; i < adjusted_global_plan.poses.size( ); ++i ) {
double g_x = adjusted_global_plan.poses[i].x;
double g_y = adjusted_global_plan.poses[i].y;
unsigned int map_x, map_y;
if ( costmap_->worldToMap(
g_x, g_y, map_x,
map_y ) && costmap_->getCost( map_x, map_y ) != nav2_costmap_2d::NO_INFORMATION )
{
int index = costmap_->getIndex( map_x, map_y );
cell_values_[index] = 0.0;
queue_->enqueueCell( map_x, map_y );
started_path = true;
} else if ( started_path ) {
break;
}
}
if ( !started_path ) {
RCLCPP_ERROR(
rclcpp::get_logger( "PathDistCritic" ),
"None of the %d first of %zu ( %zu ) points of the global plan were in "
"the local costmap and free",
i, adjusted_global_plan.poses.size( ), global_plan.poses.size( ) );
return false;
}
propogateManhattanDistances( );
return true;
}
} // namespace dwb_critics
95 PLUGINLIB_EXPORT_CLASS( dwb_critics::PathDistCritic, dwb_core::TrajectoryCritic )
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "dwb_critics/prefer_forward.hpp"
#include <math.h>
#include "pluginlib/class_list_macros.hpp"
#include "nav2_util/node_utils.hpp"
40 PLUGINLIB_EXPORT_CLASS( dwb_critics::PreferForwardCritic, dwb_core::TrajectoryCritic )
using nav2_util::declare_parameter_if_not_declared;
namespace dwb_critics
{
47 void PreferForwardCritic::onInit( )
{
auto node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
declare_parameter_if_not_declared(
node,
dwb_plugin_name_ + "." + name_ + ".penalty", rclcpp::ParameterValue( 1.0 ) );
declare_parameter_if_not_declared(
node,
dwb_plugin_name_ + "." + name_ + ".strafe_x", rclcpp::ParameterValue( 0.1 ) );
declare_parameter_if_not_declared(
node, dwb_plugin_name_ + "." + name_ + ".strafe_theta",
rclcpp::ParameterValue( 0.2 ) );
declare_parameter_if_not_declared(
node, dwb_plugin_name_ + "." + name_ + ".theta_scale",
rclcpp::ParameterValue( 10.0 ) );
node->get_parameter( dwb_plugin_name_ + "." + name_ + ".penalty", penalty_ );
node->get_parameter( dwb_plugin_name_ + "." + name_ + ".strafe_x", strafe_x_ );
node->get_parameter( dwb_plugin_name_ + "." + name_ + ".strafe_theta", strafe_theta_ );
node->get_parameter( dwb_plugin_name_ + "." + name_ + ".theta_scale", theta_scale_ );
}
73 double PreferForwardCritic::scoreTrajectory( const dwb_msgs::msg::Trajectory2D & traj )
{
// backward motions bad on a robot without backward sensors
if ( traj.velocity.x < 0.0 ) {
return penalty_;
}
// strafing motions also bad on such a robot
if ( traj.velocity.x < strafe_x_ && fabs( traj.velocity.theta ) < strafe_theta_ ) {
return penalty_;
}
// the more we rotate, the less we progress forward
return fabs( traj.velocity.theta ) * theta_scale_;
}
} // namespace dwb_critics
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "dwb_critics/rotate_to_goal.hpp"
#include <string>
#include <vector>
#include "nav_2d_utils/parameters.hpp"
#include "dwb_core/exceptions.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "dwb_core/trajectory_utils.hpp"
#include "angles/angles.h"
44 PLUGINLIB_EXPORT_CLASS( dwb_critics::RotateToGoalCritic, dwb_core::TrajectoryCritic )
namespace dwb_critics
{
inline double hypot_sq( double dx, double dy )
{
return dx * dx + dy * dy;
}
void RotateToGoalCritic::onInit( )
{
auto node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam(
node,
dwb_plugin_name_ + ".xy_goal_tolerance", 0.25 );
xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
double stopped_xy_velocity = nav_2d_utils::searchAndGetParam(
node,
dwb_plugin_name_ + ".trans_stopped_velocity", 0.25 );
stopped_xy_velocity_sq_ = stopped_xy_velocity * stopped_xy_velocity;
slowing_factor_ = nav_2d_utils::searchAndGetParam(
node,
dwb_plugin_name_ + "." + name_ + ".slowing_factor", 5.0 );
lookahead_time_ = nav_2d_utils::searchAndGetParam(
node,
dwb_plugin_name_ + "." + name_ + ".lookahead_time", -1.0 );
reset( );
}
void RotateToGoalCritic::reset( )
{
in_window_ = false;
rotating_ = false;
}
bool RotateToGoalCritic::prepare(
const geometry_msgs::msg::Pose2D & pose, const nav_2d_msgs::msg::Twist2D & vel,
const geometry_msgs::msg::Pose2D & goal,
const nav_2d_msgs::msg::Path2D & )
{
double dxy_sq = hypot_sq( pose.x - goal.x, pose.y - goal.y );
in_window_ = in_window_ || dxy_sq <= xy_goal_tolerance_sq_;
current_xy_speed_sq_ = hypot_sq( vel.x, vel.y );
rotating_ = rotating_ || ( in_window_ && current_xy_speed_sq_ <= stopped_xy_velocity_sq_ );
goal_yaw_ = goal.theta;
return true;
}
double RotateToGoalCritic::scoreTrajectory( const dwb_msgs::msg::Trajectory2D & traj )
{
// If we're not sufficiently close to the goal, we don't care what the twist is
if ( !in_window_ ) {
return 0.0;
} else if ( !rotating_ ) {
double speed_sq = hypot_sq( traj.velocity.x, traj.velocity.y );
if ( speed_sq >= current_xy_speed_sq_ ) {
throw dwb_core::IllegalTrajectoryException( name_, "Not slowing down near goal." );
}
return speed_sq * slowing_factor_ + scoreRotation( traj );
}
// If we're sufficiently close to the goal, any transforming velocity is invalid
if ( fabs( traj.velocity.x ) > 0 || fabs( traj.velocity.y ) > 0 ) {
throw dwb_core::
IllegalTrajectoryException( name_, "Nonrotation command near goal." );
}
return scoreRotation( traj );
}
double RotateToGoalCritic::scoreRotation( const dwb_msgs::msg::Trajectory2D & traj )
{
if ( traj.poses.empty( ) ) {
throw dwb_core::IllegalTrajectoryException( name_, "Empty trajectory." );
}
double end_yaw;
if ( lookahead_time_ >= 0.0 ) {
geometry_msgs::msg::Pose2D eval_pose = dwb_core::projectPose( traj, lookahead_time_ );
end_yaw = eval_pose.theta;
} else {
end_yaw = traj.poses.back( ).theta;
}
return fabs( angles::shortest_angular_distance( end_yaw, goal_yaw_ ) );
}
} // namespace dwb_critics
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "dwb_critics/twirling.hpp"
#include "pluginlib/class_list_macros.hpp"
namespace dwb_critics
{
40 void TwirlingCritic::onInit( )
{
auto node = node_.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node"};
}
// Scale is set to 0 by default, so if it was not set otherwise, set to 0
node->get_parameter( dwb_plugin_name_ + "." + name_ + ".scale", scale_ );
}
50 double TwirlingCritic::scoreTrajectory( const dwb_msgs::msg::Trajectory2D & traj )
{
return fabs( traj.velocity.theta ); // add cost for making the robot spin
}
} // namespace dwb_critics
56 PLUGINLIB_EXPORT_CLASS( dwb_critics::TwirlingCritic, dwb_core::TrajectoryCritic )
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2020, Samsung Research America
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <vector>
#include <memory>
#include <string>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "dwb_critics/alignment_util.hpp"
#include "dwb_core/exceptions.hpp"
44 TEST( AlignmentUtil, TestProjection )
{
geometry_msgs::msg::Pose2D pose, pose_out;
pose.x = 1.0;
pose.y = -1.0;
double distance = 1.0;
pose_out = dwb_critics::getForwardPose( pose, distance );
EXPECT_EQ( pose_out.x, 2.0 );
EXPECT_EQ( pose_out.y, -1.0 );
EXPECT_EQ( pose_out.theta, pose.theta );
pose.x = 2.0;
pose.y = -10.0;
pose.theta = 0.54;
pose_out = dwb_critics::getForwardPose( pose, distance );
EXPECT_NEAR( pose_out.x, 2.8577, 0.01 );
EXPECT_NEAR( pose_out.y, -9.4858, 0.01 );
EXPECT_EQ( pose_out.theta, pose.theta );
}
64 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2018, Wilco Bonestroo
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <vector>
#include <memory>
#include <string>
#include <utility>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "dwb_critics/obstacle_footprint.hpp"
#include "dwb_core/exceptions.hpp"
45 TEST( BaseObstacle, IsValidCost )
{
std::shared_ptr<dwb_critics::BaseObstacleCritic> critic =
std::make_shared<dwb_critics::BaseObstacleCritic>( );
for ( int i = 0; i < 256; i++ ) {
// for these 3 values the cost is not "valid"
if ( i == nav2_costmap_2d::LETHAL_OBSTACLE ||
i == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE ||
i == nav2_costmap_2d::NO_INFORMATION )
{
ASSERT_FALSE( critic->isValidCost( i ) );
} else {
ASSERT_TRUE( critic->isValidCost( i ) );
}
}
}
63 TEST( BaseObstacle, ScorePose )
{
std::shared_ptr<dwb_critics::BaseObstacleCritic> critic =
std::make_shared<dwb_critics::BaseObstacleCritic>( );
auto node = nav2_util::LifecycleNode::make_shared( "base_obstacle_critic_tester" );
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "test_global_costmap" );
costmap_ros->configure( );
std::string name = "name";
std::string ns = "ns";
critic->initialize( node, name, ns, costmap_ros );
costmap_ros->getCostmap( )->setCost( 0, 0, nav2_costmap_2d::LETHAL_OBSTACLE );
costmap_ros->getCostmap( )->setCost( 0, 1, nav2_costmap_2d::NO_INFORMATION );
const int some_other_cost = 128;
costmap_ros->getCostmap( )->setCost( 0, 2, some_other_cost );
// The pose is in "world" coordinates. The ( default ) resolution is 0.1 m.
geometry_msgs::msg::Pose2D pose;
pose.x = 0;
pose.y = 0;
ASSERT_THROW( critic->scorePose( pose ), dwb_core::IllegalTrajectoryException );
pose.x = 0;
pose.y = 0.15;
ASSERT_THROW( critic->scorePose( pose ), dwb_core::IllegalTrajectoryException );
pose.y = 0.25;
pose.x = 0.05;
ASSERT_EQ( critic->scorePose( pose ), some_other_cost );
// The theta should not influence the cost
for ( int i = -50; i < 150; i++ ) {
pose.theta = ( 1.0 / 50 ) * i * M_PI;
ASSERT_EQ( critic->scorePose( pose ), some_other_cost );
}
// Poses outside the map should throw an exception.
pose.x = 1.0;
pose.y = -0.1;
ASSERT_THROW( critic->scorePose( pose ), dwb_core::IllegalTrajectoryException );
pose.x = costmap_ros->getCostmap( )->getSizeInMetersX( ) + 0.1;
pose.y = 1.0;
ASSERT_THROW( critic->scorePose( pose ), dwb_core::IllegalTrajectoryException );
pose.x = 1.0;
pose.y = costmap_ros->getCostmap( )->getSizeInMetersY( ) + 0.1;
ASSERT_THROW( critic->scorePose( pose ), dwb_core::IllegalTrajectoryException );
pose.x = -0.1;
pose.y = 1.0;
ASSERT_THROW( critic->scorePose( pose ), dwb_core::IllegalTrajectoryException );
}
122 TEST( BaseObstacle, CriticVisualization )
{
std::shared_ptr<dwb_critics::BaseObstacleCritic> critic =
std::make_shared<dwb_critics::BaseObstacleCritic>( );
auto node = nav2_util::LifecycleNode::make_shared( "base_obstacle_critic_tester" );
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "test_global_costmap" );
costmap_ros->configure( );
std::string name = "name";
std::string ns = "ns";
critic->initialize( node, name, ns, costmap_ros );
costmap_ros->getCostmap( )->setCost( 0, 0, nav2_costmap_2d::LETHAL_OBSTACLE );
costmap_ros->getCostmap( )->setCost( 0, 1, nav2_costmap_2d::NO_INFORMATION );
// Some random values
costmap_ros->getCostmap( )->setCost( 3, 2, 64 );
costmap_ros->getCostmap( )->setCost( 30, 12, 85 );
costmap_ros->getCostmap( )->setCost( 10, 49, 24 );
costmap_ros->getCostmap( )->setCost( 45, 2, 12 );
std::vector<std::pair<std::string, std::vector<float>>> cost_channels;
critic->addCriticVisualization( cost_channels );
unsigned int size_x = costmap_ros->getCostmap( )->getSizeInCellsX( );
unsigned int size_y = costmap_ros->getCostmap( )->getSizeInCellsY( );
// The values in the pointcloud should be equal to the values in the costmap
for ( unsigned int y = 0; y < size_y; y++ ) {
for ( unsigned int x = 0; x < size_x; x++ ) {
float pointValue = cost_channels[0].second[y * size_y + x];
ASSERT_EQ( static_cast<int>( pointValue ), costmap_ros->getCostmap( )->getCost( x, y ) );
}
}
}
160 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2018, Wilco Bonestroo
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <vector>
#include <memory>
#include <string>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "dwb_critics/obstacle_footprint.hpp"
#include "dwb_core/exceptions.hpp"
44 class OpenObstacleFootprintCritic : public dwb_critics::ObstacleFootprintCritic
{
public:
47 double pointCost( int x, int y )
{
return dwb_critics::ObstacleFootprintCritic::pointCost( x, y );
}
52 double lineCost( int x0, int x1, int y0, int y1 )
{
return dwb_critics::ObstacleFootprintCritic::lineCost( x0, x1, y0, y1 );
}
};
// Rotate the given point for angle radians around the origin.
59 geometry_msgs::msg::Point rotate_origin( geometry_msgs::msg::Point p, double angle )
{
double s = sin( angle );
double c = cos( angle );
// rotate point
double xnew = p.x * c - p.y * s;
double ynew = p.x * s + p.y * c;
p.x = xnew;
p.y = ynew;
return p;
}
// Auxilary function to create a Point with given x and y values.
75 geometry_msgs::msg::Point getPoint( double x, double y )
{
geometry_msgs::msg::Point p;
p.x = x;
p.y = y;
return p;
}
// Variables
double footprint_size_x_half = 1.8;
double footprint_size_y_half = 1.6;
87 std::vector<geometry_msgs::msg::Point> getFootprint( )
{
std::vector<geometry_msgs::msg::Point> footprint;
footprint.push_back( getPoint( footprint_size_x_half, footprint_size_y_half ) );
footprint.push_back( getPoint( footprint_size_x_half, -footprint_size_y_half ) );
footprint.push_back( getPoint( -footprint_size_x_half, -footprint_size_y_half ) );
footprint.push_back( getPoint( -footprint_size_x_half, footprint_size_y_half ) );
return footprint;
}
97 TEST( ObstacleFootprint, GetOrientedFootprint )
{
double theta = 0.1234;
std::vector<geometry_msgs::msg::Point> footprint_before = getFootprint( );
std::vector<geometry_msgs::msg::Point> footprint_after;
geometry_msgs::msg::Pose2D pose;
pose.theta = theta;
footprint_after = dwb_critics::getOrientedFootprint( pose, footprint_before );
uint i;
for ( i = 0; i < footprint_before.size( ); i++ ) {
ASSERT_EQ( rotate_origin( footprint_before[i], theta ), footprint_after[i] );
}
theta = 5.123;
pose.theta = theta;
footprint_after = dwb_critics::getOrientedFootprint( pose, footprint_before );
for ( unsigned int i = 0; i < footprint_before.size( ); i++ ) {
ASSERT_EQ( rotate_origin( footprint_before[i], theta ), footprint_after[i] );
}
}
122 TEST( ObstacleFootprint, Prepare )
{
std::shared_ptr<dwb_critics::ObstacleFootprintCritic> critic =
std::make_shared<dwb_critics::ObstacleFootprintCritic>( );
auto node = nav2_util::LifecycleNode::make_shared( "costmap_tester" );
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "test_global_costmap" );
costmap_ros->configure( );
std::string name = "name";
std::string ns = "ns";
critic->initialize( node, name, ns, costmap_ros );
geometry_msgs::msg::Pose2D pose;
nav_2d_msgs::msg::Twist2D vel;
geometry_msgs::msg::Pose2D goal;
nav_2d_msgs::msg::Path2D global_plan;
// no footprint set in the costmap. Prepare should return false;
std::vector<geometry_msgs::msg::Point> footprint;
costmap_ros->setRobotFootprint( footprint );
ASSERT_FALSE( critic->prepare( pose, vel, goal, global_plan ) );
costmap_ros->setRobotFootprint( getFootprint( ) );
ASSERT_TRUE( critic->prepare( pose, vel, goal, global_plan ) );
double epsilon = 0.01;
// If the robot footprint goes of the map, it should throw an exception
// The following cases put the robot over the edge of the map on the left, bottom, right and top
pose.x = footprint_size_x_half; // This gives an error
pose.y = footprint_size_y_half + epsilon;
ASSERT_THROW( critic->scorePose( pose ), dwb_core::IllegalTrajectoryException );
pose.x = footprint_size_x_half + epsilon;
pose.y = footprint_size_y_half; // error
ASSERT_THROW( critic->scorePose( pose ), dwb_core::IllegalTrajectoryException );
pose.x = costmap_ros->getCostmap( )->getSizeInMetersX( ) - footprint_size_x_half; // error
pose.y = costmap_ros->getCostmap( )->getSizeInMetersY( ) + footprint_size_y_half - epsilon;
ASSERT_THROW( critic->scorePose( pose ), dwb_core::IllegalTrajectoryException );
pose.x = costmap_ros->getCostmap( )->getSizeInMetersX( ) - footprint_size_x_half - epsilon;
pose.y = costmap_ros->getCostmap( )->getSizeInMetersY( ) + footprint_size_y_half; // error
ASSERT_THROW( critic->scorePose( pose ), dwb_core::IllegalTrajectoryException );
pose.x = footprint_size_x_half + epsilon;
pose.y = footprint_size_y_half + epsilon;
ASSERT_EQ( critic->scorePose( pose ), 0.0 );
for ( unsigned int i = 1; i < costmap_ros->getCostmap( )->getSizeInCellsX( ); i++ ) {
costmap_ros->getCostmap( )->setCost( i, 10, nav2_costmap_2d::LETHAL_OBSTACLE );
}
// It should now hit an obstacle ( throw an expection )
ASSERT_THROW( critic->scorePose( pose ), dwb_core::IllegalTrajectoryException );
}
// todo: wilcobonestroo Add tests for other footprint shapes and costmaps.
182 TEST( ObstacleFootprint, PointCost )
{
std::shared_ptr<OpenObstacleFootprintCritic> critic =
std::make_shared<OpenObstacleFootprintCritic>( );
auto node = nav2_util::LifecycleNode::make_shared( "costmap_tester" );
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "test_global_costmap" );
costmap_ros->configure( );
std::string name = "name";
std::string ns = "ns";
critic->initialize( node, name, ns, costmap_ros );
costmap_ros->getCostmap( )->setCost( 0, 0, nav2_costmap_2d::LETHAL_OBSTACLE );
costmap_ros->getCostmap( )->setCost( 0, 1, nav2_costmap_2d::NO_INFORMATION );
costmap_ros->getCostmap( )->setCost( 0, 2, 128 );
ASSERT_THROW( critic->pointCost( 0, 0 ), dwb_core::IllegalTrajectoryException );
ASSERT_THROW( critic->pointCost( 0, 1 ), dwb_core::IllegalTrajectoryException );
ASSERT_EQ( critic->pointCost( 0, 2 ), 128 );
}
205 TEST( ObstacleFootprint, LineCost )
{
std::shared_ptr<OpenObstacleFootprintCritic> critic =
std::make_shared<OpenObstacleFootprintCritic>( );
auto node = nav2_util::LifecycleNode::make_shared( "costmap_tester" );
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "test_global_costmap" );
costmap_ros->configure( );
std::string name = "name";
std::string ns = "ns";
critic->initialize( node, name, ns, costmap_ros );
costmap_ros->getCostmap( )->setCost( 3, 3, nav2_costmap_2d::LETHAL_OBSTACLE );
costmap_ros->getCostmap( )->setCost( 3, 4, nav2_costmap_2d::LETHAL_OBSTACLE );
costmap_ros->getCostmap( )->setCost( 4, 3, nav2_costmap_2d::LETHAL_OBSTACLE );
costmap_ros->getCostmap( )->setCost( 4, 4, nav2_costmap_2d::LETHAL_OBSTACLE );
ASSERT_THROW( critic->lineCost( 0, 5, 2, 6 ), dwb_core::IllegalTrajectoryException );
ASSERT_THROW( critic->lineCost( 5, 0, 6, 2 ), dwb_core::IllegalTrajectoryException );
ASSERT_THROW( critic->lineCost( 2, 4, 0, 10 ), dwb_core::IllegalTrajectoryException );
ASSERT_THROW( critic->lineCost( 4, 2, 10, 0 ), dwb_core::IllegalTrajectoryException );
// These all miss the obstacle
ASSERT_EQ( critic->lineCost( 2, 2, 0, 10 ), 0.0 );
ASSERT_EQ( critic->lineCost( 2, 2, 10, 0 ), 0.0 );
ASSERT_EQ( critic->lineCost( 5, 5, 0, 10 ), 0.0 );
ASSERT_EQ( critic->lineCost( 5, 5, 10, 0 ), 0.0 );
ASSERT_EQ( critic->lineCost( 0, 50, 2, 2 ), 0.0 );
ASSERT_EQ( critic->lineCost( 50, 0, 2, 2 ), 0.0 );
ASSERT_EQ( critic->lineCost( 0, 50, 5, 5 ), 0.0 );
ASSERT_EQ( critic->lineCost( 50, 0, 5, 5 ), 0.0 );
// Use valid costs
costmap_ros->getCostmap( )->setCost( 3, 3, 50 );
costmap_ros->getCostmap( )->setCost( 3, 4, 50 );
costmap_ros->getCostmap( )->setCost( 4, 3, 100 );
costmap_ros->getCostmap( )->setCost( 4, 4, 100 );
ASSERT_EQ( critic->lineCost( 3, 3, 0, 50 ), 50 ); // all 50
ASSERT_EQ( critic->lineCost( 4, 4, 0, 10 ), 100 ); // all 100
ASSERT_EQ( critic->lineCost( 0, 50, 3, 3 ), 100 ); // pass 50 and 100
}
251 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
/*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2018, Wilco Bonestroo
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <math.h>
#include <vector>
#include <memory>
#include <string>
#include <limits>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "dwb_critics/prefer_forward.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/node_utils.hpp"
49 static constexpr double default_penalty = 1.0;
50 static constexpr double default_strafe_x = 0.1;
51 static constexpr double default_strafe_theta = 0.2;
52 static constexpr double default_theta_scale = 10.0;
TEST( PreferForward, StartNode )
{
auto critic = std::make_shared<dwb_critics::PreferForwardCritic>( );
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "test_global_costmap" );
auto node = nav2_util::LifecycleNode::make_shared( "costmap_tester" );
node->configure( );
node->activate( );
std::string name = "test";
std::string ns = "ns";
critic->initialize( node, name, ns, costmap_ros );
EXPECT_EQ( node->get_parameter( ns + "." + name + ".penalty" ).as_double( ), default_penalty );
EXPECT_EQ( node->get_parameter( ns + "." + name + ".strafe_x" ).as_double( ), default_strafe_x );
EXPECT_EQ(
node->get_parameter( ns + "." + name + ".strafe_theta" ).as_double( ), default_strafe_theta );
EXPECT_EQ( node->get_parameter( ns + "." + name + ".theta_scale" ).as_double( ), default_theta_scale );
}
TEST( PreferForward, NegativeVelocityX )
{
auto critic = std::make_shared<dwb_critics::PreferForwardCritic>( );
dwb_msgs::msg::Trajectory2D trajectory;
// score must be equal to the penalty ( 1.0 ) for any negative x velocity
trajectory.velocity.x = -1.0;
EXPECT_EQ( critic->scoreTrajectory( trajectory ), default_penalty );
trajectory.velocity.x = -0.00001;
EXPECT_EQ( critic->scoreTrajectory( trajectory ), default_penalty );
trajectory.velocity.x = -0.1;
EXPECT_EQ( critic->scoreTrajectory( trajectory ), default_penalty );
trajectory.velocity.x = std::numeric_limits<double>::lowest( );
EXPECT_EQ( critic->scoreTrajectory( trajectory ), default_penalty );
trajectory.velocity.x = -std::numeric_limits<double>::min( );
EXPECT_EQ( critic->scoreTrajectory( trajectory ), default_penalty );
}
TEST( PreferForward, Strafe )
{
auto critic = std::make_shared<dwb_critics::PreferForwardCritic>( );
dwb_msgs::msg::Trajectory2D trajectory;
// score must be equal to the penalty ( 1.0 ) when x vel is lower than 0.1
// and theta is between -0.2 and 0.2
trajectory.velocity.x = 0.05;
trajectory.velocity.theta = -0.1;
EXPECT_EQ( critic->scoreTrajectory( trajectory ), default_penalty );
trajectory.velocity.x = 0.0999999;
EXPECT_EQ( critic->scoreTrajectory( trajectory ), default_penalty );
trajectory.velocity.x = 0.000001;
EXPECT_EQ( critic->scoreTrajectory( trajectory ), default_penalty );
trajectory.velocity.theta = -0.19;
EXPECT_EQ( critic->scoreTrajectory( trajectory ), default_penalty );
trajectory.velocity.theta = 0.19;
EXPECT_EQ( critic->scoreTrajectory( trajectory ), default_penalty );
}
TEST( PreferForward, Normal )
{
auto critic = std::make_shared<dwb_critics::PreferForwardCritic>( );
dwb_msgs::msg::Trajectory2D trajectory;
// score must be equal to the theta * scaling factor ( 10.0 )
trajectory.velocity.x = 0.2;
trajectory.velocity.theta = -0.1;
EXPECT_EQ( critic->scoreTrajectory( trajectory ), 0.1 * default_theta_scale );
trajectory.velocity.theta = 0.1;
EXPECT_EQ( critic->scoreTrajectory( trajectory ), 0.1 * default_theta_scale );
trajectory.velocity.theta = -0.2;
EXPECT_EQ( critic->scoreTrajectory( trajectory ), 0.2 * default_theta_scale );
trajectory.velocity.theta = 0.2;
EXPECT_EQ( critic->scoreTrajectory( trajectory ), 0.2 * default_theta_scale );
trajectory.velocity.theta = 1.5;
EXPECT_EQ( critic->scoreTrajectory( trajectory ), 1.5 * default_theta_scale );
}
TEST( PreferForward, NoneDefaultValues )
{
auto critic = std::make_shared<dwb_critics::PreferForwardCritic>( );
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "test_global_costmap" );
auto node = nav2_util::LifecycleNode::make_shared( "costmap_tester" );
node->configure( );
node->activate( );
double penalty = 18.3;
double strafe_x = 0.5;
double strafe_theta = 0.4;
double theta_scale = 15.0;
std::string name = "test";
std::string ns = "ns";
nav2_util::declare_parameter_if_not_declared(
node, ns + "." + name + ".penalty",
rclcpp::ParameterValue( penalty ) );
nav2_util::declare_parameter_if_not_declared(
node, ns + "." + name + ".strafe_x",
rclcpp::ParameterValue( strafe_x ) );
nav2_util::declare_parameter_if_not_declared(
node, ns + "." + name + ".strafe_theta",
rclcpp::ParameterValue( strafe_theta ) );
nav2_util::declare_parameter_if_not_declared(
node, ns + "." + name + ".theta_scale",
rclcpp::ParameterValue( theta_scale ) );
critic->initialize( node, name, ns, costmap_ros );
critic->onInit( );
dwb_msgs::msg::Trajectory2D trajectory;
trajectory.velocity.x = 0.05;
trajectory.velocity.theta = -0.1;
EXPECT_EQ( critic->scoreTrajectory( trajectory ), penalty );
// score must be equal to the penalty when x vel is lower than strafe_x
// and theta is between -strafe_theta and strafe_theta
trajectory.velocity.x = 0.4;
trajectory.velocity.theta = -0.39;
EXPECT_EQ( critic->scoreTrajectory( trajectory ), penalty );
trajectory.velocity.x = 0.0999999;
EXPECT_EQ( critic->scoreTrajectory( trajectory ), penalty );
trajectory.velocity.x = 0.000001;
EXPECT_EQ( critic->scoreTrajectory( trajectory ), penalty );
trajectory.velocity.theta = -0.09999;
EXPECT_EQ( critic->scoreTrajectory( trajectory ), penalty );
trajectory.velocity.theta = 0.09999;
EXPECT_EQ( critic->scoreTrajectory( trajectory ), penalty );
// score must be equal to the theta * scaling factor ( 10.0 )
trajectory.velocity.x = 0.5;
trajectory.velocity.theta = -0.1;
EXPECT_EQ( critic->scoreTrajectory( trajectory ), 0.1 * theta_scale );
trajectory.velocity.theta = 0.1;
EXPECT_EQ( critic->scoreTrajectory( trajectory ), 0.1 * theta_scale );
trajectory.velocity.theta = -0.2;
EXPECT_EQ( critic->scoreTrajectory( trajectory ), 0.2 * theta_scale );
trajectory.velocity.theta = 0.2;
EXPECT_EQ( critic->scoreTrajectory( trajectory ), 0.2 * theta_scale );
trajectory.velocity.theta = 1.5;
EXPECT_EQ( critic->scoreTrajectory( trajectory ), 1.5 * theta_scale );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2020, Samsung Research America
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <vector>
#include <memory>
#include <string>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "dwb_critics/twirling.hpp"
#include "dwb_core/exceptions.hpp"
44 TEST( TwirlingTests, Scoring )
{
std::shared_ptr<dwb_critics::TwirlingCritic> critic =
std::make_shared<dwb_critics::TwirlingCritic>( );
auto node = nav2_util::LifecycleNode::make_shared( "costmap_tester" );
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "test_global_costmap" );
costmap_ros->configure( );
std::string name = "name";
std::string ns = "ns";
critic->initialize( node, name, ns, costmap_ros );
dwb_msgs::msg::Trajectory2D traj;
traj.velocity.theta = 1.0;
EXPECT_EQ( critic->scoreTrajectory( traj ), 1.0 );
traj.velocity.theta = -1.0;
EXPECT_EQ( critic->scoreTrajectory( traj ), 1.0 );
}
65 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
/*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_PLUGINS__KINEMATIC_PARAMETERS_HPP_
#define DWB_PLUGINS__KINEMATIC_PARAMETERS_HPP_
#include <memory>
#include <string>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "nav2_util/lifecycle_node.hpp"
namespace dwb_plugins
{
/**
* @struct KinematicParameters
* @brief A struct containing one representation of the robot's kinematics
*/
struct KinematicParameters
{
54 friend class KinematicsHandler;
inline double getMinX( ) {return min_vel_x_;}
inline double getMaxX( ) {return max_vel_x_;}
inline double getAccX( ) {return acc_lim_x_;}
inline double getDecelX( ) {return decel_lim_x_;}
inline double getMinY( ) {return min_vel_y_;}
inline double getMaxY( ) {return max_vel_y_;}
inline double getAccY( ) {return acc_lim_y_;}
inline double getDecelY( ) {return decel_lim_y_;}
inline double getMinSpeedXY( ) {return min_speed_xy_;}
inline double getMaxSpeedXY( ) {return max_speed_xy_;}
inline double getMinTheta( ) {return -max_vel_theta_;}
inline double getMaxTheta( ) {return max_vel_theta_;}
inline double getAccTheta( ) {return acc_lim_theta_;}
inline double getDecelTheta( ) {return decel_lim_theta_;}
inline double getMinSpeedTheta( ) {return min_speed_theta_;}
inline double getMinSpeedXY_SQ( ) {return min_speed_xy_sq_;}
inline double getMaxSpeedXY_SQ( ) {return max_speed_xy_sq_;}
protected:
// For parameter descriptions, see cfg/KinematicParams.cfg
double min_vel_x_{0};
double min_vel_y_{0};
double max_vel_x_{0};
double max_vel_y_{0};
double base_max_vel_x_{0};
double base_max_vel_y_{0};
double max_vel_theta_{0};
double base_max_vel_theta_{0};
double min_speed_xy_{0};
double max_speed_xy_{0};
double base_max_speed_xy_{0};
double min_speed_theta_{0};
double acc_lim_x_{0};
double acc_lim_y_{0};
double acc_lim_theta_{0};
double decel_lim_x_{0};
double decel_lim_y_{0};
double decel_lim_theta_{0};
// Cached square values of min_speed_xy and max_speed_xy
double min_speed_xy_sq_{0};
double max_speed_xy_sq_{0};
};
/**
* @class KinematicsHandler
* @brief A class managing the representation of the robot's kinematics
*/
class KinematicsHandler
{
public:
KinematicsHandler( );
~KinematicsHandler( );
void initialize( const nav2_util::LifecycleNode::SharedPtr & nh, const std::string & plugin_name );
inline KinematicParameters getKinematics( ) {return *kinematics_.load( );}
void setSpeedLimit( const double & speed_limit, const bool & percentage );
using Ptr = std::shared_ptr<KinematicsHandler>;
protected:
std::atomic<KinematicParameters *> kinematics_;
// Dynamic parameters handler
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
/**
* @brief Callback executed when a paramter change is detected
* @param parameters list of changed parameters
*/
rcl_interfaces::msg::SetParametersResult
dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters );
void update_kinematics( KinematicParameters kinematics );
std::string plugin_name_;
};
} // namespace dwb_plugins
#endif // DWB_PLUGINS__KINEMATIC_PARAMETERS_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_PLUGINS__LIMITED_ACCEL_GENERATOR_HPP_
#define DWB_PLUGINS__LIMITED_ACCEL_GENERATOR_HPP_
#include <memory>
#include <string>
#include "dwb_plugins/standard_traj_generator.hpp"
#include "nav2_util/lifecycle_node.hpp"
namespace dwb_plugins
{
/**
* @class LimitedAccelGenerator
* @brief Limits the acceleration in the generated trajectories to a fraction of the simulated time.
*/
50 class LimitedAccelGenerator : public StandardTrajectoryGenerator
{
public:
void initialize(
const nav2_util::LifecycleNode::SharedPtr & nh,
const std::string & plugin_name ) override;
void startNewIteration( const nav_2d_msgs::msg::Twist2D & current_velocity ) override;
protected:
/**
* @brief Calculate the velocity after a set period of time, given the desired velocity and acceleration limits
*
* Unlike the StandardTrajectoryGenerator, the velocity remains constant in the LimitedAccelGenerator
*
* @param cmd_vel Desired velocity
* @param start_vel starting velocity
* @param dt amount of time in seconds
* @return cmd_vel
*/
nav_2d_msgs::msg::Twist2D computeNewVelocity(
const nav_2d_msgs::msg::Twist2D & cmd_vel,
const nav_2d_msgs::msg::Twist2D & start_vel,
const double dt ) override;
double acceleration_time_;
std::string plugin_name_;
};
} // namespace dwb_plugins
#endif // DWB_PLUGINS__LIMITED_ACCEL_GENERATOR_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_PLUGINS__ONE_D_VELOCITY_ITERATOR_HPP_
#define DWB_PLUGINS__ONE_D_VELOCITY_ITERATOR_HPP_
#include <algorithm>
#include <cmath>
namespace dwb_plugins
{
const double EPSILON = 1E-5;
/**
* @brief Given initial conditions and a time, figure out the end velocity
*
* @param v0 Initial velocity
* @param accel The acceleration rate
* @param decel The decceleration rate
* @param dt Delta time - amount of time to project into the future
* @param target target velocity
* @return The velocity dt seconds after v0.
*/
56 inline double projectVelocity( double v0, double accel, double decel, double dt, double target )
{
double v1;
if ( v0 < target ) {
v1 = v0 + accel * dt;
return std::min( target, v1 );
} else {
v1 = v0 + decel * dt;
return std::max( target, v1 );
}
}
/**
* @class OneDVelocityIterator
* @brief An iterator for generating a number of samples in a range
*
* In its simplest usage, this gives us N ( num_samples ) different velocities that are reachable
* given our current velocity. However, there is some fancy logic around zero velocities and
* the min/max velocities
*
* If the current velocity is 2 m/s, and the acceleration limit is 1 m/ss and the acc_time is 1 s,
* this class would provide velocities between 1 m/s and 3 m/s.
*
*
*
*/
82 class OneDVelocityIterator
{
public:
/**
* @brief Constructor for the velocity iterator
*
* @param current Current velocity
* @param min Minimum velocity allowable
* @param max Maximum velocity allowable
* @param acc_limit Acceleration Limit
* @param decel_limit Deceleration Limit
* @param num_samples The number of samples to return
*/
95 OneDVelocityIterator(
double current, double min, double max, double acc_limit, double decel_limit, double acc_time,
int num_samples )
{
if ( current < min ) {
current = min;
} else if ( current > max ) {
current = max;
}
max_vel_ = projectVelocity( current, acc_limit, decel_limit, acc_time, max );
min_vel_ = projectVelocity( current, acc_limit, decel_limit, acc_time, min );
reset( );
if ( fabs( min_vel_ - max_vel_ ) < EPSILON ) {
increment_ = 1.0;
return;
}
num_samples = std::max( 2, num_samples );
// e.g. for 4 samples, split distance in 3 even parts
increment_ = ( max_vel_ - min_vel_ ) / std::max( 1, ( num_samples - 1 ) );
}
/**
* @brief Get the next velocity available
*/
121 double getVelocity( ) const
{
if ( return_zero_now_ ) {return 0.0;}
return current_;
}
/**
* @brief Increment the iterator
*/
130 OneDVelocityIterator & operator++( )
{
if ( return_zero_ && current_ < 0.0 && current_ + increment_ > 0.0 &&
current_ + increment_ <= max_vel_ + EPSILON )
{
return_zero_now_ = true;
return_zero_ = false;
} else {
current_ += increment_;
return_zero_now_ = false;
}
return *this;
}
/**
* @brief Reset back to the first velocity
*/
147 void reset( )
{
current_ = min_vel_;
return_zero_ = true;
return_zero_now_ = false;
}
/**
* If we have returned all the velocities for this iteration
*/
157 bool isFinished( ) const
{
return current_ > max_vel_ + EPSILON;
}
private:
163 bool return_zero_, return_zero_now_;
double min_vel_, max_vel_;
double current_;
double increment_;
};
} // namespace dwb_plugins
#endif // DWB_PLUGINS__ONE_D_VELOCITY_ITERATOR_HPP_
/*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_PLUGINS__STANDARD_TRAJ_GENERATOR_HPP_
#define DWB_PLUGINS__STANDARD_TRAJ_GENERATOR_HPP_
#include <vector>
#include <memory>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "dwb_core/trajectory_generator.hpp"
#include "dwb_plugins/velocity_iterator.hpp"
#include "dwb_plugins/kinematic_parameters.hpp"
#include "nav2_util/lifecycle_node.hpp"
namespace dwb_plugins
{
/**
* @class StandardTrajectoryGenerator
* @brief Standard DWA-like trajectory generator.
*/
55 class StandardTrajectoryGenerator : public dwb_core::TrajectoryGenerator
{
public:
// Standard TrajectoryGenerator interface
void initialize(
const nav2_util::LifecycleNode::SharedPtr & nh,
const std::string & plugin_name ) override;
void startNewIteration( const nav_2d_msgs::msg::Twist2D & current_velocity ) override;
bool hasMoreTwists( ) override;
nav_2d_msgs::msg::Twist2D nextTwist( ) override;
dwb_msgs::msg::Trajectory2D generateTrajectory(
const geometry_msgs::msg::Pose2D & start_pose,
const nav_2d_msgs::msg::Twist2D & start_vel,
const nav_2d_msgs::msg::Twist2D & cmd_vel ) override;
/**
* @brief Limits the maximum linear speed of the robot.
* @param speed_limit expressed in absolute value ( in m/s )
* or in percentage from maximum robot speed.
* @param percentage Setting speed limit in percentage if true
* or in absolute values in false case.
*/
void setSpeedLimit( const double & speed_limit, const bool & percentage ) override
{
if ( kinematics_handler_ ) {
kinematics_handler_->setSpeedLimit( speed_limit, percentage );
}
}
protected:
/**
* @brief Initialize the VelocityIterator pointer. Put in its own function for easy overriding
*/
virtual void initializeIterator( const nav2_util::LifecycleNode::SharedPtr & nh );
/**
* @brief Calculate the velocity after a set period of time, given the desired velocity and acceleration limits
*
* @param cmd_vel Desired velocity
* @param start_vel starting velocity
* @param dt amount of time in seconds
* @return new velocity after dt seconds
*/
99 virtual nav_2d_msgs::msg::Twist2D computeNewVelocity(
const nav_2d_msgs::msg::Twist2D & cmd_vel, const nav_2d_msgs::msg::Twist2D & start_vel,
const double dt );
/**
* @brief Use the robot's kinematic model to predict new positions for the robot
*
* @param start_pose Starting pose
* @param vel Actual robot velocity ( assumed to be within acceleration limits )
* @param dt amount of time in seconds
* @return New pose after dt seconds
*/
virtual geometry_msgs::msg::Pose2D computeNewPosition(
const geometry_msgs::msg::Pose2D start_pose, const nav_2d_msgs::msg::Twist2D & vel,
const double dt );
/**
* @brief Compute an array of time deltas between the points in the generated trajectory.
*
* @param cmd_vel The desired command velocity
* @return vector of the difference between each time step in the generated trajectory
*
* If we are discretizing by time, the returned vector will be the same constant time_granularity
* for all cmd_vels. Otherwise, you will get times based on the linear/angular granularity.
*
* Right now the vector contains a single value repeated many times, but this method could be overridden
* to allow for dynamic spacing
*/
virtual std::vector<double> getTimeSteps( const nav_2d_msgs::msg::Twist2D & cmd_vel );
KinematicsHandler::Ptr kinematics_handler_;
std::shared_ptr<VelocityIterator> velocity_iterator_;
double sim_time_;
// Sampling Parameters
bool discretize_by_time_;
/// @brief If discretizing by time, the amount of time between each point in the traj
double time_granularity_;
/// @brief If not discretizing by time, the amount of linear space between points
double linear_granularity_;
/// @brief If not discretizing by time, the amount of angular space between points
double angular_granularity_;
/// @brief the name of the overlying plugin ID
std::string plugin_name_;
/* Backwards Compatibility Parameter: include_last_point
*
* dwa had an off-by-one error built into it.
* It generated N trajectory points, where N = ceil( sim_time / time_delta ).
* If for example, sim_time=3.0 and time_delta=1.5, it would generate trajectories with 2 points, which
* indeed were time_delta seconds apart. However, the points would be at t=0 and t=1.5, and thus the
* actual sim_time was much less than advertised.
*
* This is remedied by adding one final point at t=sim_time, but only if include_last_point_ is true.
*
* Nothing I could find actually used the time_delta variable or seemed to care that the trajectories
* were not projected out as far as they intended.
*/
bool include_last_point_;
};
} // namespace dwb_plugins
#endif // DWB_PLUGINS__STANDARD_TRAJ_GENERATOR_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_PLUGINS__VELOCITY_ITERATOR_HPP_
#define DWB_PLUGINS__VELOCITY_ITERATOR_HPP_
#include <memory>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "nav_2d_msgs/msg/twist2_d.hpp"
#include "dwb_plugins/kinematic_parameters.hpp"
#include "nav2_util/lifecycle_node.hpp"
namespace dwb_plugins
{
48 class VelocityIterator
{
public:
51 virtual ~VelocityIterator( ) {}
52 virtual void initialize(
53 const nav2_util::LifecycleNode::SharedPtr & nh,
54 KinematicsHandler::Ptr kinematics,
55 const std::string & plugin_name ) = 0;
56 virtual void startNewIteration( const nav_2d_msgs::msg::Twist2D & current_velocity, double dt ) = 0;
57 virtual bool hasMoreTwists( ) = 0;
58 virtual nav_2d_msgs::msg::Twist2D nextTwist( ) = 0;
};
} // namespace dwb_plugins
#endif // DWB_PLUGINS__VELOCITY_ITERATOR_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWB_PLUGINS__XY_THETA_ITERATOR_HPP_
#define DWB_PLUGINS__XY_THETA_ITERATOR_HPP_
#include <memory>
#include <string>
#include "dwb_plugins/velocity_iterator.hpp"
#include "dwb_plugins/one_d_velocity_iterator.hpp"
#include "nav2_util/lifecycle_node.hpp"
namespace dwb_plugins
{
47 class XYThetaIterator : public VelocityIterator
{
public:
50 XYThetaIterator( )
: kinematics_handler_( nullptr ), x_it_( nullptr ), y_it_( nullptr ), th_it_( nullptr ) {}
void initialize(
const nav2_util::LifecycleNode::SharedPtr & nh,
KinematicsHandler::Ptr kinematics,
const std::string & plugin_name ) override;
void startNewIteration( const nav_2d_msgs::msg::Twist2D & current_velocity, double dt ) override;
bool hasMoreTwists( ) override;
nav_2d_msgs::msg::Twist2D nextTwist( ) override;
protected:
/**
* @brief Check to see whether the combined x/y/theta velocities are valid
* @return True if the magnitude hypot( x, y ) and theta are within the robot's absolute limits
*
* This is based on three parameters: min_speed_xy, max_speed_xy and min_speed_theta.
* The speed is valid if
* 1 ) The combined magnitude hypot( x, y ) is less than max_speed_xy ( or max_speed_xy is negative )
* AND
* 2 ) min_speed_xy is negative or min_speed_theta is negative or
* hypot( x, y ) is greater than min_speed_xy or fabs( theta ) is greater than min_speed_theta.
*/
bool isValidSpeed( double x, double y, double theta );
virtual bool isValidVelocity( );
void iterateToValidVelocity( );
int vx_samples_, vy_samples_, vtheta_samples_;
KinematicsHandler::Ptr kinematics_handler_;
std::shared_ptr<OneDVelocityIterator> x_it_, y_it_, th_it_;
};
} // namespace dwb_plugins
#endif // DWB_PLUGINS__XY_THETA_ITERATOR_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "dwb_plugins/kinematic_parameters.hpp"
#include <memory>
#include <string>
#include <vector>
#include "nav_2d_utils/parameters.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
using nav2_util::declare_parameter_if_not_declared;
using rcl_interfaces::msg::ParameterType;
using std::placeholders::_1;
namespace dwb_plugins
{
52 KinematicsHandler::KinematicsHandler( )
{
kinematics_.store( new KinematicParameters );
}
57 KinematicsHandler::~KinematicsHandler( )
{
delete kinematics_.load( );
}
62 void KinematicsHandler::initialize(
63 const nav2_util::LifecycleNode::SharedPtr & nh,
64 const std::string & plugin_name )
{
plugin_name_ = plugin_name;
declare_parameter_if_not_declared( nh, plugin_name + ".min_vel_x", rclcpp::ParameterValue( 0.0 ) );
declare_parameter_if_not_declared( nh, plugin_name + ".min_vel_y", rclcpp::ParameterValue( 0.0 ) );
declare_parameter_if_not_declared( nh, plugin_name + ".max_vel_x", rclcpp::ParameterValue( 0.0 ) );
declare_parameter_if_not_declared( nh, plugin_name + ".max_vel_y", rclcpp::ParameterValue( 0.0 ) );
declare_parameter_if_not_declared(
nh, plugin_name + ".max_vel_theta",
rclcpp::ParameterValue( 0.0 ) );
declare_parameter_if_not_declared(
nh, plugin_name + ".min_speed_xy",
rclcpp::ParameterValue( 0.0 ) );
declare_parameter_if_not_declared(
nh, plugin_name + ".max_speed_xy",
rclcpp::ParameterValue( 0.0 ) );
declare_parameter_if_not_declared(
nh, plugin_name + ".min_speed_theta",
rclcpp::ParameterValue( 0.0 ) );
declare_parameter_if_not_declared( nh, plugin_name + ".acc_lim_x", rclcpp::ParameterValue( 0.0 ) );
declare_parameter_if_not_declared( nh, plugin_name + ".acc_lim_y", rclcpp::ParameterValue( 0.0 ) );
declare_parameter_if_not_declared(
nh, plugin_name + ".acc_lim_theta",
rclcpp::ParameterValue( 0.0 ) );
declare_parameter_if_not_declared( nh, plugin_name + ".decel_lim_x", rclcpp::ParameterValue( 0.0 ) );
declare_parameter_if_not_declared( nh, plugin_name + ".decel_lim_y", rclcpp::ParameterValue( 0.0 ) );
declare_parameter_if_not_declared(
nh, plugin_name + ".decel_lim_theta",
rclcpp::ParameterValue( 0.0 ) );
KinematicParameters kinematics;
nh->get_parameter( plugin_name + ".min_vel_x", kinematics.min_vel_x_ );
nh->get_parameter( plugin_name + ".min_vel_y", kinematics.min_vel_y_ );
nh->get_parameter( plugin_name + ".max_vel_x", kinematics.max_vel_x_ );
nh->get_parameter( plugin_name + ".max_vel_y", kinematics.max_vel_y_ );
nh->get_parameter( plugin_name + ".max_vel_theta", kinematics.max_vel_theta_ );
nh->get_parameter( plugin_name + ".min_speed_xy", kinematics.min_speed_xy_ );
nh->get_parameter( plugin_name + ".max_speed_xy", kinematics.max_speed_xy_ );
nh->get_parameter( plugin_name + ".min_speed_theta", kinematics.min_speed_theta_ );
nh->get_parameter( plugin_name + ".acc_lim_x", kinematics.acc_lim_x_ );
nh->get_parameter( plugin_name + ".acc_lim_y", kinematics.acc_lim_y_ );
nh->get_parameter( plugin_name + ".acc_lim_theta", kinematics.acc_lim_theta_ );
nh->get_parameter( plugin_name + ".decel_lim_x", kinematics.decel_lim_x_ );
nh->get_parameter( plugin_name + ".decel_lim_y", kinematics.decel_lim_y_ );
nh->get_parameter( plugin_name + ".decel_lim_theta", kinematics.decel_lim_theta_ );
kinematics.base_max_vel_x_ = kinematics.max_vel_x_;
kinematics.base_max_vel_y_ = kinematics.max_vel_y_;
kinematics.base_max_speed_xy_ = kinematics.max_speed_xy_;
kinematics.base_max_vel_theta_ = kinematics.max_vel_theta_;
// Add callback for dynamic parameters
dyn_params_handler_ = nh->add_on_set_parameters_callback(
std::bind( &KinematicsHandler::dynamicParametersCallback, this, _1 ) );
kinematics.min_speed_xy_sq_ = kinematics.min_speed_xy_ * kinematics.min_speed_xy_;
kinematics.max_speed_xy_sq_ = kinematics.max_speed_xy_ * kinematics.max_speed_xy_;
update_kinematics( kinematics );
}
127 void KinematicsHandler::setSpeedLimit(
128 const double & speed_limit, const bool & percentage )
{
KinematicParameters kinematics( *kinematics_.load( ) );
if ( speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT ) {
// Restore default value
kinematics.max_speed_xy_ = kinematics.base_max_speed_xy_;
kinematics.max_vel_x_ = kinematics.base_max_vel_x_;
kinematics.max_vel_y_ = kinematics.base_max_vel_y_;
kinematics.max_vel_theta_ = kinematics.base_max_vel_theta_;
} else {
if ( percentage ) {
// Speed limit is expressed in % from maximum speed of robot
kinematics.max_speed_xy_ = kinematics.base_max_speed_xy_ * speed_limit / 100.0;
kinematics.max_vel_x_ = kinematics.base_max_vel_x_ * speed_limit / 100.0;
kinematics.max_vel_y_ = kinematics.base_max_vel_y_ * speed_limit / 100.0;
kinematics.max_vel_theta_ = kinematics.base_max_vel_theta_ * speed_limit / 100.0;
} else {
// Speed limit is expressed in absolute value
if ( speed_limit < kinematics.base_max_speed_xy_ ) {
kinematics.max_speed_xy_ = speed_limit;
// Handling components and angular velocity changes:
// Max velocities are being changed in the same proportion
// as absolute linear speed changed in order to preserve
// robot moving trajectories to be the same after speed change.
const double ratio = speed_limit / kinematics.base_max_speed_xy_;
kinematics.max_vel_x_ = kinematics.base_max_vel_x_ * ratio;
kinematics.max_vel_y_ = kinematics.base_max_vel_y_ * ratio;
kinematics.max_vel_theta_ = kinematics.base_max_vel_theta_ * ratio;
}
}
}
// Do not forget to update max_speed_xy_sq_ as well
kinematics.max_speed_xy_sq_ = kinematics.max_speed_xy_ * kinematics.max_speed_xy_;
update_kinematics( kinematics );
}
rcl_interfaces::msg::SetParametersResult
168 KinematicsHandler::dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters )
{
rcl_interfaces::msg::SetParametersResult result;
KinematicParameters kinematics( *kinematics_.load( ) );
for ( auto parameter : parameters ) {
const auto & type = parameter.get_type( );
const auto & name = parameter.get_name( );
if ( type == ParameterType::PARAMETER_DOUBLE ) {
if ( name == plugin_name_ + ".min_vel_x" ) {
kinematics.min_vel_x_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".min_vel_y" ) {
kinematics.min_vel_y_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".max_vel_x" ) {
kinematics.max_vel_x_ = parameter.as_double( );
kinematics.base_max_vel_x_ = kinematics.max_vel_x_;
} else if ( name == plugin_name_ + ".max_vel_y" ) {
kinematics.max_vel_y_ = parameter.as_double( );
kinematics.base_max_vel_y_ = kinematics.max_vel_y_;
} else if ( name == plugin_name_ + ".max_vel_theta" ) {
kinematics.max_vel_theta_ = parameter.as_double( );
kinematics.base_max_vel_theta_ = kinematics.max_vel_theta_;
} else if ( name == plugin_name_ + ".min_speed_xy" ) {
kinematics.min_speed_xy_ = parameter.as_double( );
kinematics.min_speed_xy_sq_ = kinematics.min_speed_xy_ * kinematics.min_speed_xy_;
} else if ( name == plugin_name_ + ".max_speed_xy" ) {
kinematics.max_speed_xy_ = parameter.as_double( );
kinematics.base_max_speed_xy_ = kinematics.max_speed_xy_;
} else if ( name == plugin_name_ + ".min_speed_theta" ) {
kinematics.min_speed_theta_ = parameter.as_double( );
kinematics.max_speed_xy_sq_ = kinematics.max_speed_xy_ * kinematics.max_speed_xy_;
} else if ( name == plugin_name_ + ".acc_lim_x" ) {
kinematics.acc_lim_x_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".acc_lim_y" ) {
kinematics.acc_lim_y_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".acc_lim_theta" ) {
kinematics.acc_lim_theta_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".decel_lim_x" ) {
kinematics.decel_lim_x_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".decel_lim_y" ) {
kinematics.decel_lim_y_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".decel_lim_theta" ) {
kinematics.decel_lim_theta_ = parameter.as_double( );
}
}
}
update_kinematics( kinematics );
result.successful = true;
return result;
}
220 void KinematicsHandler::update_kinematics( KinematicParameters kinematics )
{
delete kinematics_.load( );
kinematics_.store( new KinematicParameters( kinematics ) );
}
} // namespace dwb_plugins
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "dwb_plugins/limited_accel_generator.hpp"
#include <vector>
#include <memory>
#include <string>
#include "nav_2d_utils/parameters.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "dwb_core/exceptions.hpp"
#include "nav2_util/node_utils.hpp"
namespace dwb_plugins
{
47 void LimitedAccelGenerator::initialize(
48 const nav2_util::LifecycleNode::SharedPtr & nh,
49 const std::string & plugin_name )
{
plugin_name_ = plugin_name;
StandardTrajectoryGenerator::initialize( nh, plugin_name_ );
try {
nav2_util::declare_parameter_if_not_declared(
nh, plugin_name + ".sim_period", rclcpp::PARAMETER_DOUBLE );
if ( !nh->get_parameter( plugin_name + ".sim_period", acceleration_time_ ) ) {
// This actually should never appear, since declare_parameter_if_not_declared( )
// completed w/o exceptions guarantee that static parameter will be initialized
// with some value. However for reliability we should also process the case
// when get_parameter( ) will return a failure for some other reasons.
throw std::runtime_error( "Failed to get 'sim_period' value" );
}
} catch ( std::exception & ) {
RCLCPP_WARN(
rclcpp::get_logger( "LimitedAccelGenerator" ),
"'sim_period' parameter is not set for %s", plugin_name.c_str( ) );
double controller_frequency = nav_2d_utils::searchAndGetParam(
nh, "controller_frequency", 20.0 );
if ( controller_frequency > 0 ) {
acceleration_time_ = 1.0 / controller_frequency;
} else {
RCLCPP_WARN(
rclcpp::get_logger( "LimitedAccelGenerator" ),
"A controller_frequency less than or equal to 0 has been set. "
"Ignoring the parameter, assuming a rate of 20Hz" );
acceleration_time_ = 0.05;
}
}
}
82 void LimitedAccelGenerator::startNewIteration( const nav_2d_msgs::msg::Twist2D & current_velocity )
{
// Limit our search space to just those within the limited acceleration_time
velocity_iterator_->startNewIteration( current_velocity, acceleration_time_ );
}
88 nav_2d_msgs::msg::Twist2D LimitedAccelGenerator::computeNewVelocity(
89 const nav_2d_msgs::msg::Twist2D & cmd_vel,
90 const nav_2d_msgs::msg::Twist2D & /*start_vel*/,
const double /*dt*/ )
{
return cmd_vel;
}
} // namespace dwb_plugins
98 PLUGINLIB_EXPORT_CLASS( dwb_plugins::LimitedAccelGenerator, dwb_core::TrajectoryGenerator )
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "dwb_plugins/standard_traj_generator.hpp"
#include <string>
#include <vector>
#include <algorithm>
#include <memory>
#include "dwb_plugins/xy_theta_iterator.hpp"
#include "nav_2d_utils/parameters.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "dwb_core/exceptions.hpp"
#include "nav2_util/node_utils.hpp"
namespace dwb_plugins
{
49 void StandardTrajectoryGenerator::initialize(
50 const nav2_util::LifecycleNode::SharedPtr & nh,
51 const std::string & plugin_name )
{
plugin_name_ = plugin_name;
kinematics_handler_ = std::make_shared<KinematicsHandler>( );
kinematics_handler_->initialize( nh, plugin_name_ );
initializeIterator( nh );
nav2_util::declare_parameter_if_not_declared(
nh,
plugin_name + ".sim_time", rclcpp::ParameterValue( 1.7 ) );
nav2_util::declare_parameter_if_not_declared(
nh,
plugin_name + ".discretize_by_time", rclcpp::ParameterValue( false ) );
nav2_util::declare_parameter_if_not_declared(
nh,
plugin_name + ".time_granularity", rclcpp::ParameterValue( 0.5 ) );
nav2_util::declare_parameter_if_not_declared(
nh,
plugin_name + ".linear_granularity", rclcpp::ParameterValue( 0.5 ) );
nav2_util::declare_parameter_if_not_declared(
nh,
plugin_name + ".angular_granularity", rclcpp::ParameterValue( 0.025 ) );
nav2_util::declare_parameter_if_not_declared(
nh,
plugin_name + ".include_last_point", rclcpp::ParameterValue( true ) );
/*
* If discretize_by_time, then sim_granularity represents the amount of time that should be between
* two successive points on the trajectory.
*
* If discretize_by_time is false, then sim_granularity is the maximum amount of distance between
* two successive points on the trajectory, and angular_sim_granularity is the maximum amount of
* angular distance between two successive points.
*/
nh->get_parameter( plugin_name + ".sim_time", sim_time_ );
nh->get_parameter( plugin_name + ".discretize_by_time", discretize_by_time_ );
nh->get_parameter( plugin_name + ".time_granularity", time_granularity_ );
nh->get_parameter( plugin_name + ".linear_granularity", linear_granularity_ );
nh->get_parameter( plugin_name + ".angular_granularity", angular_granularity_ );
nh->get_parameter( plugin_name + ".include_last_point", include_last_point_ );
}
94 void StandardTrajectoryGenerator::initializeIterator(
95 const nav2_util::LifecycleNode::SharedPtr & nh )
{
velocity_iterator_ = std::make_shared<XYThetaIterator>( );
velocity_iterator_->initialize( nh, kinematics_handler_, plugin_name_ );
}
101 void StandardTrajectoryGenerator::startNewIteration(
102 const nav_2d_msgs::msg::Twist2D & current_velocity )
{
velocity_iterator_->startNewIteration( current_velocity, sim_time_ );
}
107 bool StandardTrajectoryGenerator::hasMoreTwists( )
{
return velocity_iterator_->hasMoreTwists( );
}
112 nav_2d_msgs::msg::Twist2D StandardTrajectoryGenerator::nextTwist( )
{
return velocity_iterator_->nextTwist( );
}
117 std::vector<double> StandardTrajectoryGenerator::getTimeSteps(
118 const nav_2d_msgs::msg::Twist2D & cmd_vel )
{
std::vector<double> steps;
if ( discretize_by_time_ ) {
steps.resize( ceil( sim_time_ / time_granularity_ ) );
} else { // discretize by distance
double vmag = hypot( cmd_vel.x, cmd_vel.y );
// the distance the robot would travel in sim_time if it did not change velocity
double projected_linear_distance = vmag * sim_time_;
// the angle the robot would rotate in sim_time
double projected_angular_distance = fabs( cmd_vel.theta ) * sim_time_;
// Pick the maximum of the two
int num_steps = ceil(
std::max(
projected_linear_distance / linear_granularity_,
projected_angular_distance / angular_granularity_ ) );
steps.resize( num_steps );
}
if ( steps.size( ) == 0 ) {
steps.resize( 1 );
}
std::fill( steps.begin( ), steps.end( ), sim_time_ / steps.size( ) );
return steps;
}
146 dwb_msgs::msg::Trajectory2D StandardTrajectoryGenerator::generateTrajectory(
147 const geometry_msgs::msg::Pose2D & start_pose,
148 const nav_2d_msgs::msg::Twist2D & start_vel,
149 const nav_2d_msgs::msg::Twist2D & cmd_vel )
{
dwb_msgs::msg::Trajectory2D traj;
traj.velocity = cmd_vel;
// simulate the trajectory
geometry_msgs::msg::Pose2D pose = start_pose;
nav_2d_msgs::msg::Twist2D vel = start_vel;
double running_time = 0.0;
std::vector<double> steps = getTimeSteps( cmd_vel );
traj.poses.push_back( start_pose );
for ( double dt : steps ) {
// calculate velocities
vel = computeNewVelocity( cmd_vel, vel, dt );
// update the position of the robot using the velocities passed in
pose = computeNewPosition( pose, vel, dt );
traj.poses.push_back( pose );
traj.time_offsets.push_back( rclcpp::Duration::from_seconds( running_time ) );
running_time += dt;
} // end for simulation steps
if ( include_last_point_ ) {
traj.poses.push_back( pose );
traj.time_offsets.push_back( rclcpp::Duration::from_seconds( running_time ) );
}
return traj;
}
/**
* change vel using acceleration limits to converge towards sample_target-vel
*/
182 nav_2d_msgs::msg::Twist2D StandardTrajectoryGenerator::computeNewVelocity(
183 const nav_2d_msgs::msg::Twist2D & cmd_vel,
184 const nav_2d_msgs::msg::Twist2D & start_vel, const double dt )
{
KinematicParameters kinematics = kinematics_handler_->getKinematics( );
nav_2d_msgs::msg::Twist2D new_vel;
new_vel.x = projectVelocity(
start_vel.x, kinematics.getAccX( ),
kinematics.getDecelX( ), dt, cmd_vel.x );
new_vel.y = projectVelocity(
start_vel.y, kinematics.getAccY( ),
kinematics.getDecelY( ), dt, cmd_vel.y );
new_vel.theta = projectVelocity(
start_vel.theta,
kinematics.getAccTheta( ), kinematics.getDecelTheta( ),
dt, cmd_vel.theta );
return new_vel;
}
201 geometry_msgs::msg::Pose2D StandardTrajectoryGenerator::computeNewPosition(
202 const geometry_msgs::msg::Pose2D start_pose,
203 const nav_2d_msgs::msg::Twist2D & vel, const double dt )
{
geometry_msgs::msg::Pose2D new_pose;
new_pose.x = start_pose.x +
( vel.x * cos( start_pose.theta ) + vel.y * cos( M_PI_2 + start_pose.theta ) ) * dt;
new_pose.y = start_pose.y +
( vel.x * sin( start_pose.theta ) + vel.y * sin( M_PI_2 + start_pose.theta ) ) * dt;
new_pose.theta = start_pose.theta + vel.theta * dt;
return new_pose;
}
} // namespace dwb_plugins
216 PLUGINLIB_EXPORT_CLASS(
dwb_plugins::StandardTrajectoryGenerator,
dwb_core::TrajectoryGenerator )
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "dwb_plugins/xy_theta_iterator.hpp"
#include <cmath>
#include <memory>
#include <string>
#include "nav_2d_utils/parameters.hpp"
#include "nav2_util/node_utils.hpp"
#define EPSILON 1E-5
namespace dwb_plugins
{
48 void XYThetaIterator::initialize(
49 const nav2_util::LifecycleNode::SharedPtr & nh,
50 KinematicsHandler::Ptr kinematics,
51 const std::string & plugin_name )
{
kinematics_handler_ = kinematics;
nav2_util::declare_parameter_if_not_declared(
nh,
plugin_name + ".vx_samples", rclcpp::ParameterValue( 20 ) );
nav2_util::declare_parameter_if_not_declared(
nh,
plugin_name + ".vy_samples", rclcpp::ParameterValue( 5 ) );
nav2_util::declare_parameter_if_not_declared(
nh,
plugin_name + ".vtheta_samples", rclcpp::ParameterValue( 20 ) );
nh->get_parameter( plugin_name + ".vx_samples", vx_samples_ );
nh->get_parameter( plugin_name + ".vy_samples", vy_samples_ );
nh->get_parameter( plugin_name + ".vtheta_samples", vtheta_samples_ );
}
70 void XYThetaIterator::startNewIteration(
71 const nav_2d_msgs::msg::Twist2D & current_velocity,
double dt )
{
KinematicParameters kinematics = kinematics_handler_->getKinematics( );
x_it_ = std::make_shared<OneDVelocityIterator>(
current_velocity.x,
kinematics.getMinX( ), kinematics.getMaxX( ),
kinematics.getAccX( ), kinematics.getDecelX( ),
dt, vx_samples_ );
y_it_ = std::make_shared<OneDVelocityIterator>(
current_velocity.y,
kinematics.getMinY( ), kinematics.getMaxY( ),
kinematics.getAccY( ), kinematics.getDecelY( ),
dt, vy_samples_ );
th_it_ = std::make_shared<OneDVelocityIterator>(
current_velocity.theta,
kinematics.getMinTheta( ), kinematics.getMaxTheta( ),
kinematics.getAccTheta( ), kinematics.getDecelTheta( ),
dt, vtheta_samples_ );
if ( !isValidVelocity( ) ) {
iterateToValidVelocity( );
}
}
95 bool XYThetaIterator::isValidSpeed( double x, double y, double theta )
{
KinematicParameters kinematics = kinematics_handler_->getKinematics( );
double vmag_sq = x * x + y * y;
if ( kinematics.getMaxSpeedXY( ) >= 0.0 && vmag_sq > kinematics.getMaxSpeedXY_SQ( ) + EPSILON ) {
return false;
}
if ( kinematics.getMinSpeedXY( ) >= 0.0 && vmag_sq + EPSILON < kinematics.getMinSpeedXY_SQ( ) &&
kinematics.getMinSpeedTheta( ) >= 0.0 && fabs( theta ) + EPSILON < kinematics.getMinSpeedTheta( ) )
{
return false;
}
if ( vmag_sq == 0.0 && th_it_->getVelocity( ) == 0.0 ) {
return false;
}
return true;
}
113 bool XYThetaIterator::isValidVelocity( )
{
return isValidSpeed(
x_it_->getVelocity( ), y_it_->getVelocity( ),
th_it_->getVelocity( ) );
}
120 bool XYThetaIterator::hasMoreTwists( )
{
return x_it_ && !x_it_->isFinished( );
}
125 nav_2d_msgs::msg::Twist2D XYThetaIterator::nextTwist( )
{
nav_2d_msgs::msg::Twist2D velocity;
velocity.x = x_it_->getVelocity( );
velocity.y = y_it_->getVelocity( );
velocity.theta = th_it_->getVelocity( );
iterateToValidVelocity( );
return velocity;
}
137 void XYThetaIterator::iterateToValidVelocity( )
{
bool valid = false;
while ( !valid && hasMoreTwists( ) ) {
++( *th_it_ );
if ( th_it_->isFinished( ) ) {
th_it_->reset( );
++( *y_it_ );
if ( y_it_->isFinished( ) ) {
y_it_->reset( );
++( *x_it_ );
}
}
valid = isValidVelocity( );
}
}
} // namespace dwb_plugins
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2018, Wilco Bonestroo
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <string>
#include <memory>
#include <vector>
#include "gtest/gtest.h"
#include "dwb_plugins/kinematic_parameters.hpp"
using rcl_interfaces::msg::Parameter;
using rcl_interfaces::msg::ParameterType;
using rcl_interfaces::msg::ParameterEvent;
46 class KinematicsHandlerTest : public dwb_plugins::KinematicsHandler
{
public:
49 void simulate_event(
50 std::vector<rclcpp::Parameter> parameters )
{
dynamicParametersCallback( parameters );
}
};
56 TEST( KinematicParameters, SetAllParameters ) {
std::string nodeName = "test_node";
auto node = nav2_util::LifecycleNode::make_shared( nodeName );
KinematicsHandlerTest kh;
kh.initialize( node, nodeName );
std::vector<rclcpp::Parameter> parameters;
rclcpp::Parameter
p_minX( nodeName + ".min_vel_x", 12.34 ),
p_maxX( nodeName + ".max_vel_x", 23.45 ),
p_minY( nodeName + ".min_vel_y", 34.56 ),
p_maxY( nodeName + ".max_vel_y", 45.67 ),
p_accX( nodeName + ".acc_lim_x", 56.78 ),
p_decelX( nodeName + ".acc_lim_y", 67.89 ),
p_accY( nodeName + ".decel_lim_x", 78.90 ),
p_decelY( nodeName + ".decel_lim_y", 89.01 ),
p_minSpeedXY( nodeName + ".min_speed_xy", 90.12 ),
p_maxSpeedXY( nodeName + ".max_speed_xy", 123.456 ),
p_maxTheta( nodeName + ".max_vel_theta", 345.678 ),
p_accTheta( nodeName + ".acc_lim_theta", 234.567 ),
p_decelTheta( nodeName + ".decel_lim_theta", 456.789 ),
p_minSpeedTheta( nodeName + ".min_speed_theta", 567.890 );
parameters.push_back( p_minX );
parameters.push_back( p_minX );
parameters.push_back( p_maxX );
parameters.push_back( p_minY );
parameters.push_back( p_maxY );
parameters.push_back( p_accX );
parameters.push_back( p_accY );
parameters.push_back( p_decelX );
parameters.push_back( p_decelY );
parameters.push_back( p_minSpeedXY );
parameters.push_back( p_maxSpeedXY );
parameters.push_back( p_maxTheta );
parameters.push_back( p_accTheta );
parameters.push_back( p_decelTheta );
parameters.push_back( p_minSpeedTheta );
kh.simulate_event( parameters );
dwb_plugins::KinematicParameters kp = kh.getKinematics( );
EXPECT_EQ( kp.getMinX( ), 12.34 );
EXPECT_EQ( kp.getMaxX( ), 23.45 );
EXPECT_EQ( kp.getMinY( ), 34.56 );
EXPECT_EQ( kp.getMaxY( ), 45.67 );
EXPECT_EQ( kp.getAccX( ), 56.78 );
EXPECT_EQ( kp.getAccY( ), 67.89 );
EXPECT_EQ( kp.getDecelX( ), 78.90 );
EXPECT_EQ( kp.getDecelY( ), 89.01 );
EXPECT_EQ( kp.getMinSpeedXY( ), 90.12 );
EXPECT_EQ( kp.getMaxSpeedXY( ), 123.456 );
EXPECT_EQ( kp.getAccTheta( ), 234.567 );
EXPECT_EQ( kp.getMaxTheta( ), 345.678 );
EXPECT_EQ( kp.getDecelTheta( ), 456.789 );
EXPECT_EQ( kp.getMinSpeedTheta( ), 567.890 );
}
116 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2020, Samsung Research Russia
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Alexey Merzlyakov
*/
#include <gtest/gtest.h>
#include <string>
#include <memory>
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
#include "dwb_plugins/kinematic_parameters.hpp"
using namespace std::chrono_literals;
51 static constexpr double EPSILON = 1e-5;
static const char NODE_NAME[] = "test_node";
static const double MAX_VEL_X = 40.0;
static const double MAX_VEL_Y = 30.0;
static const double MAX_VEL_THETA = 15.0;
static const double MAX_VEL_LINEAR = 50.0;
59 class TestNode : public ::testing::Test
{
public:
62 TestNode( )
{
const std::string node_name = NODE_NAME;
node_ = nav2_util::LifecycleNode::make_shared( node_name );
node_->declare_parameter(
node_name + ".max_vel_x", rclcpp::ParameterValue( MAX_VEL_X ) );
node_->set_parameter(
rclcpp::Parameter( node_name + ".max_vel_x", MAX_VEL_X ) );
node_->declare_parameter(
node_name + ".max_vel_y", rclcpp::ParameterValue( MAX_VEL_Y ) );
node_->set_parameter(
rclcpp::Parameter( node_name + ".max_vel_y", MAX_VEL_Y ) );
node_->declare_parameter(
node_name + ".max_vel_theta", rclcpp::ParameterValue( MAX_VEL_THETA ) );
node_->set_parameter(
rclcpp::Parameter( node_name + ".max_vel_theta", MAX_VEL_THETA ) );
node_->declare_parameter(
node_name + ".max_speed_xy", rclcpp::ParameterValue( MAX_VEL_LINEAR ) );
node_->set_parameter(
rclcpp::Parameter( node_name + ".max_speed_xy", MAX_VEL_LINEAR ) );
}
88 ~TestNode( ) {}
protected:
91 nav2_util::LifecycleNode::SharedPtr node_;
};
94 TEST_F( TestNode, TestPercentLimit )
{
dwb_plugins::KinematicsHandler kh;
kh.initialize( node_, NODE_NAME );
dwb_plugins::KinematicParameters kp = kh.getKinematics( );
EXPECT_NEAR( kp.getMaxX( ), MAX_VEL_X, EPSILON );
EXPECT_NEAR( kp.getMaxY( ), MAX_VEL_Y, EPSILON );
EXPECT_NEAR( kp.getMaxTheta( ), MAX_VEL_THETA, EPSILON );
EXPECT_NEAR( kp.getMaxSpeedXY( ), MAX_VEL_LINEAR, EPSILON );
// Set speed limit 30% from maximum robot speed
kh.setSpeedLimit( 30, true );
// Update KinematicParameters values from KinematicsHandler
kp = kh.getKinematics( );
EXPECT_NEAR( kp.getMaxX( ), MAX_VEL_X * 0.3, EPSILON );
EXPECT_NEAR( kp.getMaxY( ), MAX_VEL_Y * 0.3, EPSILON );
EXPECT_NEAR( kp.getMaxTheta( ), MAX_VEL_THETA * 0.3, EPSILON );
EXPECT_NEAR( kp.getMaxSpeedXY( ), MAX_VEL_LINEAR * 0.3, EPSILON );
// Restore maximum speed to its default
kh.setSpeedLimit( nav2_costmap_2d::NO_SPEED_LIMIT, true );
// Update KinematicParameters values from KinematicsHandler
kp = kh.getKinematics( );
EXPECT_NEAR( kp.getMaxX( ), MAX_VEL_X, EPSILON );
EXPECT_NEAR( kp.getMaxY( ), MAX_VEL_Y, EPSILON );
EXPECT_NEAR( kp.getMaxTheta( ), MAX_VEL_THETA, EPSILON );
EXPECT_NEAR( kp.getMaxSpeedXY( ), MAX_VEL_LINEAR, EPSILON );
}
126 TEST_F( TestNode, TestAbsoluteLimit )
{
dwb_plugins::KinematicsHandler kh;
kh.initialize( node_, NODE_NAME );
dwb_plugins::KinematicParameters kp = kh.getKinematics( );
EXPECT_NEAR( kp.getMaxX( ), MAX_VEL_X, EPSILON );
EXPECT_NEAR( kp.getMaxY( ), MAX_VEL_Y, EPSILON );
EXPECT_NEAR( kp.getMaxTheta( ), MAX_VEL_THETA, EPSILON );
EXPECT_NEAR( kp.getMaxSpeedXY( ), MAX_VEL_LINEAR, EPSILON );
// Set speed limit 35.0 m/s
kh.setSpeedLimit( 35.0, false );
// Update KinematicParameters values from KinematicsHandler
kp = kh.getKinematics( );
EXPECT_NEAR( kp.getMaxX( ), MAX_VEL_X * 35.0 / MAX_VEL_LINEAR, EPSILON );
EXPECT_NEAR( kp.getMaxY( ), MAX_VEL_Y * 35.0 / MAX_VEL_LINEAR, EPSILON );
EXPECT_NEAR( kp.getMaxTheta( ), MAX_VEL_THETA * 35.0 / MAX_VEL_LINEAR, EPSILON );
EXPECT_NEAR( kp.getMaxSpeedXY( ), 35.0, EPSILON );
// Restore maximum speed to its default
kh.setSpeedLimit( nav2_costmap_2d::NO_SPEED_LIMIT, false );
// Update KinematicParameters values from KinematicsHandler
kp = kh.getKinematics( );
EXPECT_NEAR( kp.getMaxX( ), MAX_VEL_X, EPSILON );
EXPECT_NEAR( kp.getMaxY( ), MAX_VEL_Y, EPSILON );
EXPECT_NEAR( kp.getMaxTheta( ), MAX_VEL_THETA, EPSILON );
EXPECT_NEAR( kp.getMaxSpeedXY( ), MAX_VEL_LINEAR, EPSILON );
}
158 int main( int argc, char ** argv )
{
// Initialize the system
testing::InitGoogleTest( &argc, argv );
rclcpp::init( argc, argv );
// Actual testing
bool test_result = RUN_ALL_TESTS( );
// Shutdown
rclcpp::shutdown( );
return test_result;
}
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <cmath>
#include <vector>
#include <algorithm>
#include <string>
#include "gtest/gtest.h"
#include "dwb_plugins/standard_traj_generator.hpp"
#include "dwb_plugins/limited_accel_generator.hpp"
#include "dwb_core/exceptions.hpp"
#include "nav2_util/node_utils.hpp"
using std::hypot;
using std::fabs;
using dwb_plugins::StandardTrajectoryGenerator;
geometry_msgs::msg::Pose2D origin;
nav_2d_msgs::msg::Twist2D zero;
nav_2d_msgs::msg::Twist2D forward;
54 class LimitedAccelGeneratorTest : public dwb_plugins::LimitedAccelGenerator
{
public:
57 double getAccelerationTime( )
{
return acceleration_time_;
}
};
63 std::vector<rclcpp::Parameter> getDefaultKinematicParameters( )
{
std::vector<rclcpp::Parameter> parameters;
parameters.push_back( rclcpp::Parameter( "dwb.min_vel_x", 0.0 ) );
parameters.push_back( rclcpp::Parameter( "dwb.max_vel_x", 0.55 ) );
parameters.push_back( rclcpp::Parameter( "dwb.min_vel_y", -0.1 ) );
parameters.push_back( rclcpp::Parameter( "dwb.max_vel_y", 0.1 ) );
parameters.push_back( rclcpp::Parameter( "dwb.max_vel_theta", 1.0 ) );
parameters.push_back( rclcpp::Parameter( "dwb.acc_lim_x", 2.5 ) );
parameters.push_back( rclcpp::Parameter( "dwb.acc_lim_y", 2.5 ) );
parameters.push_back( rclcpp::Parameter( "dwb.acc_lim_theta", 3.2 ) );
parameters.push_back( rclcpp::Parameter( "dwb.decel_lim_x", -2.5 ) );
parameters.push_back( rclcpp::Parameter( "dwb.decel_lim_y", -2.5 ) );
parameters.push_back( rclcpp::Parameter( "dwb.decel_lim_theta", -3.2 ) );
parameters.push_back( rclcpp::Parameter( "dwb.min_speed_xy", 0.1 ) );
parameters.push_back( rclcpp::Parameter( "dwb.max_speed_xy", 0.55 ) );
parameters.push_back( rclcpp::Parameter( "dwb.min_speed_theta", 0.4 ) );
return parameters;
}
86 rclcpp_lifecycle::LifecycleNode::SharedPtr makeTestNode(
const std::string & name,
const std::vector<rclcpp::Parameter> & overrides = {} )
{
rclcpp::NodeOptions node_options;
node_options.parameter_overrides( getDefaultKinematicParameters( ) );
node_options.parameter_overrides( ).insert(
node_options.parameter_overrides( ).end( ), overrides.begin( ), overrides.end( ) );
auto node = rclcpp_lifecycle::LifecycleNode::make_shared( name, node_options );
node->on_configure( node->get_current_state( ) );
node->on_activate( node->get_current_state( ) );
return node;
}
102 void checkLimits(
const std::vector<nav_2d_msgs::msg::Twist2D> & twists,
double exp_min_x, double exp_max_x, double exp_min_y, double exp_max_y,
double exp_min_theta, double exp_max_theta,
double exp_max_xy = -1.0,
double exp_min_xy = -1.0, double exp_min_speed_theta = -1.0 )
{
ASSERT_GT( twists.size( ), 0u );
nav_2d_msgs::msg::Twist2D first = twists[0];
double min_x = first.x, max_x = first.x, min_y = first.y, max_y = first.y;
double min_theta = first.theta, max_theta = first.theta;
double max_xy = hypot( first.x, first.y );
for ( nav_2d_msgs::msg::Twist2D twist : twists ) {
min_x = std::min( min_x, twist.x );
min_y = std::min( min_y, twist.y );
min_theta = std::min( min_theta, twist.theta );
max_x = std::max( max_x, twist.x );
max_y = std::max( max_y, twist.y );
max_theta = std::max( max_theta, twist.theta );
double hyp = hypot( twist.x, twist.y );
max_xy = std::max( max_xy, hyp );
if ( exp_min_xy >= 0 && exp_min_speed_theta >= 0 ) {
EXPECT_TRUE( fabs( twist.theta ) >= exp_min_speed_theta || hyp >= exp_min_xy );
}
}
EXPECT_DOUBLE_EQ( min_x, exp_min_x );
EXPECT_DOUBLE_EQ( max_x, exp_max_x );
EXPECT_DOUBLE_EQ( min_y, exp_min_y );
EXPECT_DOUBLE_EQ( max_y, exp_max_y );
EXPECT_DOUBLE_EQ( min_theta, exp_min_theta );
EXPECT_DOUBLE_EQ( max_theta, exp_max_theta );
if ( exp_max_xy >= 0 ) {
EXPECT_DOUBLE_EQ( max_xy, exp_max_xy );
}
}
141 double durationToSec( builtin_interfaces::msg::Duration d )
{
return d.sec + d.nanosec * 1e-9;
}
146 TEST( VelocityIterator, standard_gen )
{
auto nh = makeTestNode( "st_gen" );
StandardTrajectoryGenerator gen;
gen.initialize( nh, "dwb" );
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists( zero );
EXPECT_EQ( twists.size( ), 1926u );
checkLimits( twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, 0.55, 0.1, 0.4 );
}
156 TEST( VelocityIterator, max_xy )
{
auto nh = makeTestNode( "max_xy", {rclcpp::Parameter( "dwb.max_speed_xy", 1.0 )} );
StandardTrajectoryGenerator gen;
gen.initialize( nh, "dwb" );
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists( zero );
// Expect more twists since max_speed_xy is now beyond feasible limits
EXPECT_EQ( twists.size( ), 2010u );
checkLimits( twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, hypot( 0.55, 0.1 ) );
}
168 TEST( VelocityIterator, min_xy )
{
auto nh = makeTestNode( "min_xy", {rclcpp::Parameter( "dwb.min_speed_xy", -1.0 )} );
StandardTrajectoryGenerator gen;
gen.initialize( nh, "dwb" );
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists( zero );
// Expect even more since theres no min_speed_xy
EXPECT_EQ( twists.size( ), 2015u );
checkLimits( twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0 );
}
179 TEST( VelocityIterator, min_theta )
{
auto nh = makeTestNode( "min_theta", {rclcpp::Parameter( "dwb.min_speed_theta", -1.0 )} );
StandardTrajectoryGenerator gen;
gen.initialize( nh, "dwb" );
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists( zero );
// Expect even more since theres no min_speed_xy
EXPECT_EQ( twists.size( ), 2015u );
checkLimits( twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0 );
}
190 TEST( VelocityIterator, no_limits )
{
auto nh = makeTestNode(
"no_limits", {
rclcpp::Parameter( "dwb.max_speed_xy", -1.0 ),
rclcpp::Parameter( "dwb.min_speed_xy", -1.0 ),
rclcpp::Parameter( "dwb.min_speed_theta", -1.0 )} );
StandardTrajectoryGenerator gen;
gen.initialize( nh, "dwb" );
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists( zero );
// vx_samples * vtheta_samples * vy_samples + added zero theta samples - ( 0, 0, 0 )
EXPECT_EQ( twists.size( ), 20u * 20u * 5u + 100u - 1u );
checkLimits( twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, hypot( 0.55, 0.1 ), 0.0, 0.0 );
}
205 TEST( VelocityIterator, no_limits_samples )
{
const int x_samples = 10, y_samples = 3, theta_samples = 5;
auto nh = makeTestNode(
"no_limits_samples", {
rclcpp::Parameter( "dwb.max_speed_xy", -1.0 ),
rclcpp::Parameter( "dwb.min_speed_xy", -1.0 ),
rclcpp::Parameter( "dwb.min_speed_theta", -1.0 ),
rclcpp::Parameter( "dwb.vx_samples", x_samples ),
rclcpp::Parameter( "dwb.vy_samples", y_samples ),
rclcpp::Parameter( "dwb.vtheta_samples", theta_samples )} );
StandardTrajectoryGenerator gen;
gen.initialize( nh, "dwb" );
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists( zero );
EXPECT_EQ( twists.size( ), static_cast<unsigned>( x_samples * y_samples * theta_samples - 1 ) );
checkLimits( twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, hypot( 0.55, 0.1 ), 0.0, 0.0 );
}
223 TEST( VelocityIterator, dwa_gen )
{
auto nh = makeTestNode( "dwa_gen", {rclcpp::Parameter( "dwb.min_speed_theta", -1.0 )} );
dwb_plugins::LimitedAccelGenerator gen;
gen.initialize( nh, "dwb" );
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists( zero );
// Same as no-limits since everything is within our velocity limits
EXPECT_EQ( twists.size( ), 20u * 20u * 5u + 100u - 1u );
checkLimits( twists, 0.0, 0.125, -0.1, 0.1, -0.16, 0.16, hypot( 0.125, 0.1 ), 0.0, 0.1 );
}
234 TEST( VelocityIterator, dwa_gen_zero_frequency )
{
auto nh = makeTestNode( "dwa_gen" );
nh->declare_parameter( "controller_frequency", 0.0 );
LimitedAccelGeneratorTest gen;
gen.initialize( nh, "dwb" );
// Default value should be 0.05
EXPECT_EQ( gen.getAccelerationTime( ), 0.05 );
}
244 TEST( VelocityIterator, dwa_gen_one_frequency )
{
auto nh = makeTestNode( "dwa_gen" );
nh->declare_parameter( "controller_frequency", 1.0 );
LimitedAccelGeneratorTest gen;
gen.initialize( nh, "dwb" );
EXPECT_EQ( gen.getAccelerationTime( ), 1.0 );
}
253 TEST( VelocityIterator, dwa_gen_ten_frequency )
{
auto nh = makeTestNode( "dwa_gen" );
nh->declare_parameter( "controller_frequency", 10.0 );
LimitedAccelGeneratorTest gen;
gen.initialize( nh, "dwb" );
EXPECT_EQ( gen.getAccelerationTime( ), 0.1 );
}
262 TEST( VelocityIterator, dwa_gen_fifty_frequency )
{
auto nh = makeTestNode( "dwa_gen" );
nh->declare_parameter( "controller_frequency", 50.0 );
LimitedAccelGeneratorTest gen;
gen.initialize( nh, "dwb" );
EXPECT_EQ( gen.getAccelerationTime( ), 0.02 );
}
271 TEST( VelocityIterator, dwa_gen_hundred_frequency )
{
auto nh = makeTestNode( "dwa_gen" );
nh->declare_parameter( "controller_frequency", 100.0 );
LimitedAccelGeneratorTest gen;
gen.initialize( nh, "dwb" );
EXPECT_EQ( gen.getAccelerationTime( ), 0.01 );
}
280 TEST( VelocityIterator, nonzero )
{
auto nh = makeTestNode( "nonzero", {rclcpp::Parameter( "dwb.min_speed_theta", -1.0 )} );
dwb_plugins::LimitedAccelGenerator gen;
gen.initialize( nh, "dwb" );
nav_2d_msgs::msg::Twist2D initial;
initial.x = 0.1;
initial.y = -0.08;
initial.theta = 0.05;
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists( initial );
EXPECT_EQ( twists.size( ), 2519u );
checkLimits(
twists, 0.0, 0.225, -0.1, 0.045, -0.11000000000000003, 0.21,
0.24622144504490268, 0.0, 0.1 );
}
296 void matchPose( const geometry_msgs::msg::Pose2D & a, const geometry_msgs::msg::Pose2D & b )
{
EXPECT_DOUBLE_EQ( a.x, b.x );
EXPECT_DOUBLE_EQ( a.y, b.y );
EXPECT_DOUBLE_EQ( a.theta, b.theta );
}
303 void matchPose(
const geometry_msgs::msg::Pose2D & a, const double x, const double y,
const double theta )
{
EXPECT_DOUBLE_EQ( a.x, x );
EXPECT_DOUBLE_EQ( a.y, y );
EXPECT_DOUBLE_EQ( a.theta, theta );
}
312 void matchTwist( const nav_2d_msgs::msg::Twist2D & a, const nav_2d_msgs::msg::Twist2D & b )
{
EXPECT_DOUBLE_EQ( a.x, b.x );
EXPECT_DOUBLE_EQ( a.y, b.y );
EXPECT_DOUBLE_EQ( a.theta, b.theta );
}
319 void matchTwist(
const nav_2d_msgs::msg::Twist2D & a, const double x, const double y,
const double theta )
{
EXPECT_DOUBLE_EQ( a.x, x );
EXPECT_DOUBLE_EQ( a.y, y );
EXPECT_DOUBLE_EQ( a.theta, theta );
}
const double DEFAULT_SIM_TIME = 1.7;
330 TEST( TrajectoryGenerator, basic )
{
auto nh = makeTestNode( "basic", {rclcpp::Parameter( "dwb.linear_granularity", 0.5 )} );
StandardTrajectoryGenerator gen;
gen.initialize( nh, "dwb" );
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory( origin, forward, forward );
matchTwist( res.velocity, forward );
EXPECT_DOUBLE_EQ( durationToSec( res.time_offsets.back( ) ), DEFAULT_SIM_TIME );
int n = res.poses.size( );
EXPECT_EQ( n, 4 );
ASSERT_GT( n, 0 );
matchPose( res.poses[0], origin );
matchPose( res.poses[n - 1], DEFAULT_SIM_TIME * forward.x, 0, 0 );
}
346 TEST( TrajectoryGenerator, basic_no_last_point )
{
auto nh = makeTestNode(
"basic_no_last_point", {
rclcpp::Parameter( "dwb.include_last_point", false ),
rclcpp::Parameter( "dwb.linear_granularity", 0.5 )} );
StandardTrajectoryGenerator gen;
gen.initialize( nh, "dwb" );
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory( origin, forward, forward );
matchTwist( res.velocity, forward );
EXPECT_DOUBLE_EQ( durationToSec( res.time_offsets.back( ) ), DEFAULT_SIM_TIME / 2 );
int n = res.poses.size( );
EXPECT_EQ( n, 3 );
ASSERT_GT( n, 0 );
matchPose( res.poses[0], origin );
matchPose( res.poses[n - 2], 0.255, 0, 0 );
}
365 TEST( TrajectoryGenerator, too_slow )
{
auto nh = makeTestNode( "too_slow", {rclcpp::Parameter( "dwb.linear_granularity", 0.5 )} );
StandardTrajectoryGenerator gen;
gen.initialize( nh, "dwb" );
nav_2d_msgs::msg::Twist2D cmd;
cmd.x = 0.2;
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory( origin, cmd, cmd );
matchTwist( res.velocity, cmd );
EXPECT_DOUBLE_EQ( durationToSec( res.time_offsets.back( ) ), DEFAULT_SIM_TIME );
int n = res.poses.size( );
EXPECT_EQ( n, 3 );
ASSERT_GT( n, 0 );
matchPose( res.poses[0], origin );
}
382 TEST( TrajectoryGenerator, holonomic )
{
auto nh = makeTestNode( "holonomic", {rclcpp::Parameter( "dwb.linear_granularity", 0.5 )} );
StandardTrajectoryGenerator gen;
gen.initialize( nh, "dwb" );
nav_2d_msgs::msg::Twist2D cmd;
cmd.x = 0.3;
cmd.y = 0.2;
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory( origin, cmd, cmd );
matchTwist( res.velocity, cmd );
EXPECT_DOUBLE_EQ( durationToSec( res.time_offsets.back( ) ), DEFAULT_SIM_TIME );
int n = res.poses.size( );
EXPECT_EQ( n, 4 );
ASSERT_GT( n, 0 );
matchPose( res.poses[0], origin );
matchPose( res.poses[n - 1], cmd.x * DEFAULT_SIM_TIME, cmd.y * DEFAULT_SIM_TIME, 0 );
}
401 TEST( TrajectoryGenerator, twisty )
{
auto nh = makeTestNode(
"twisty", {
rclcpp::Parameter( "dwb.linear_granularity", 0.5 ),
rclcpp::Parameter( "dwb.angular_granularity", 0.025 )} );
StandardTrajectoryGenerator gen;
gen.initialize( nh, "dwb" );
nav_2d_msgs::msg::Twist2D cmd;
cmd.x = 0.3;
cmd.y = -0.2;
cmd.theta = 0.111;
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory( origin, cmd, cmd );
matchTwist( res.velocity, cmd );
EXPECT_NEAR( durationToSec( res.time_offsets.back( ) ), DEFAULT_SIM_TIME, 1.0E-5 );
int n = res.poses.size( );
EXPECT_EQ( n, 10 );
ASSERT_GT( n, 0 );
matchPose( res.poses[0], origin );
matchPose(
res.poses[n - 1], 0.5355173615993063, -0.29635287789821596,
cmd.theta * DEFAULT_SIM_TIME );
}
426 TEST( TrajectoryGenerator, sim_time )
{
const double sim_time = 2.5;
auto nh = makeTestNode(
"sim_time", {
rclcpp::Parameter( "dwb.sim_time", sim_time ),
rclcpp::Parameter( "dwb.linear_granularity", 0.5 )} );
StandardTrajectoryGenerator gen;
gen.initialize( nh, "dwb" );
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory( origin, forward, forward );
matchTwist( res.velocity, forward );
EXPECT_DOUBLE_EQ( durationToSec( res.time_offsets.back( ) ), sim_time );
int n = res.poses.size( );
EXPECT_EQ( n, 4 );
ASSERT_GT( n, 0 );
matchPose( res.poses[0], origin );
matchPose( res.poses[n - 2], sim_time * forward.x, 0, 0 );
}
446 TEST( TrajectoryGenerator, accel )
{
auto nh = makeTestNode(
"accel", {
rclcpp::Parameter( "dwb.sim_time", 5.0 ),
rclcpp::Parameter( "dwb.discretize_by_time", true ),
rclcpp::Parameter( "dwb.time_granularity", 1.0 ),
rclcpp::Parameter( "dwb.acc_lim_x", 0.1 ),
rclcpp::Parameter( "dwb.min_speed_xy", -1.0 )} );
StandardTrajectoryGenerator gen;
gen.initialize( nh, "dwb" );
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory( origin, zero, forward );
matchTwist( res.velocity, forward );
EXPECT_DOUBLE_EQ( durationToSec( res.time_offsets.back( ) ), 5.0 );
ASSERT_EQ( res.poses.size( ), 7u );
matchPose( res.poses[0], origin );
matchPose( res.poses[1], 0.1, 0, 0 );
matchPose( res.poses[2], 0.3, 0, 0 );
matchPose( res.poses[3], 0.6, 0, 0 );
matchPose( res.poses[4], 0.9, 0, 0 );
matchPose( res.poses[5], 1.2, 0, 0 );
}
470 TEST( TrajectoryGenerator, dwa )
{
auto nh = makeTestNode(
"dwa", {
rclcpp::Parameter( "dwb.sim_period", 1.0 ),
rclcpp::Parameter( "dwb.sim_time", 5.0 ),
rclcpp::Parameter( "dwb.discretize_by_time", true ),
rclcpp::Parameter( "dwb.time_granularity", 1.0 ),
rclcpp::Parameter( "dwb.acc_lim_x", 0.1 ),
rclcpp::Parameter( "dwb.min_speed_xy", -1.0 )} );
dwb_plugins::LimitedAccelGenerator gen;
gen.initialize( nh, "dwb" );
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory( origin, zero, forward );
matchTwist( res.velocity, forward );
EXPECT_DOUBLE_EQ( durationToSec( res.time_offsets.back( ) ), 5.0 );
ASSERT_EQ( res.poses.size( ), 7u );
matchPose( res.poses[0], origin );
matchPose( res.poses[1], 0.3, 0, 0 );
matchPose( res.poses[2], 0.6, 0, 0 );
matchPose( res.poses[3], 0.9, 0, 0 );
matchPose( res.poses[4], 1.2, 0, 0 );
matchPose( res.poses[5], 1.5, 0, 0 );
}
495 int main( int argc, char ** argv )
{
forward.x = 0.3;
rclcpp::init( 0, nullptr );
testing::InitGoogleTest( &argc, argv );
int ret = RUN_ALL_TESTS( );
rclcpp::shutdown( );
return ret;
}
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "gtest/gtest.h"
#include "dwb_plugins/one_d_velocity_iterator.hpp"
using dwb_plugins::OneDVelocityIterator;
const double EPSILON = 1e-3;
42 TEST( VelocityIterator, basics )
{
OneDVelocityIterator it( 2.0, 0.0, 5.0, 1.0, -1.0, 1.0, 2 );
EXPECT_FALSE( it.isFinished( ) );
EXPECT_NEAR( it.getVelocity( ), 1.0, EPSILON );
EXPECT_FALSE( it.isFinished( ) );
++it;
EXPECT_FALSE( it.isFinished( ) );
EXPECT_NEAR( it.getVelocity( ), 3.0, EPSILON );
EXPECT_FALSE( it.isFinished( ) );
++it;
EXPECT_TRUE( it.isFinished( ) );
it.reset( );
EXPECT_FALSE( it.isFinished( ) );
EXPECT_NEAR( it.getVelocity( ), 1.0, EPSILON );
}
59 TEST( VelocityIterator, limits )
{
OneDVelocityIterator it( 2.0, 1.5, 2.5, 1.0, -1.0, 1.0, 2 );
EXPECT_NEAR( it.getVelocity( ), 1.5, EPSILON );
++it;
EXPECT_NEAR( it.getVelocity( ), 2.5, EPSILON );
}
67 TEST( VelocityIterator, acceleration )
{
OneDVelocityIterator it( 2.0, 0.0, 5.0, 0.5, -0.5, 1.0, 2 );
EXPECT_NEAR( it.getVelocity( ), 1.5, EPSILON );
++it;
EXPECT_NEAR( it.getVelocity( ), 2.5, EPSILON );
}
76 TEST( VelocityIterator, time )
{
OneDVelocityIterator it( 2.0, 0.0, 5.0, 1.0, -1.0, 0.5, 2 );
EXPECT_NEAR( it.getVelocity( ), 1.5, EPSILON );
++it;
EXPECT_NEAR( it.getVelocity( ), 2.5, EPSILON );
}
84 TEST( VelocityIterator, samples )
{
OneDVelocityIterator it( 2.0, 0.0, 5.0, 1.0, -1.0, 1.0, 3 );
EXPECT_NEAR( it.getVelocity( ), 1.0, EPSILON );
++it;
EXPECT_NEAR( it.getVelocity( ), 2.0, EPSILON );
++it;
EXPECT_NEAR( it.getVelocity( ), 3.0, EPSILON );
++it;
EXPECT_TRUE( it.isFinished( ) );
}
97 TEST( VelocityIterator, samples2 )
{
OneDVelocityIterator it( 2.0, 0.0, 5.0, 1.0, -1.0, 1.0, 5 );
EXPECT_NEAR( it.getVelocity( ), 1.0, EPSILON );
++it;
EXPECT_NEAR( it.getVelocity( ), 1.5, EPSILON );
++it;
EXPECT_NEAR( it.getVelocity( ), 2.0, EPSILON );
++it;
EXPECT_NEAR( it.getVelocity( ), 2.5, EPSILON );
++it;
EXPECT_NEAR( it.getVelocity( ), 3.0, EPSILON );
++it;
EXPECT_TRUE( it.isFinished( ) );
}
113 TEST( VelocityIterator, around_zero )
{
OneDVelocityIterator it( 0.0, -5.0, 5.0, 1.0, -1.0, 1.0, 2 );
EXPECT_NEAR( it.getVelocity( ), -1.0, EPSILON );
++it;
EXPECT_NEAR( it.getVelocity( ), 0.0, EPSILON );
++it;
EXPECT_NEAR( it.getVelocity( ), 1.0, EPSILON );
++it;
}
124 TEST( VelocityIterator, around_zero2 )
{
OneDVelocityIterator it( 0.0, -5.0, 5.0, 1.0, -1.0, 1.0, 4 );
EXPECT_NEAR( it.getVelocity( ), -1.0, EPSILON );
++it;
EXPECT_NEAR( it.getVelocity( ), -0.3333, EPSILON );
++it;
EXPECT_NEAR( it.getVelocity( ), 0.0, EPSILON );
++it;
EXPECT_NEAR( it.getVelocity( ), 0.3333, EPSILON );
++it;
EXPECT_NEAR( it.getVelocity( ), 1.0, EPSILON );
++it;
}
139 int main( int argc, char ** argv )
{
testing::InitGoogleTest( &argc, argv );
return RUN_ALL_TESTS( );
}
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef NAV_2D_UTILS__CONVERSIONS_HPP_
#define NAV_2D_UTILS__CONVERSIONS_HPP_
#include <vector>
#include <string>
#include "geometry_msgs/msg/pose.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "nav_2d_msgs/msg/twist2_d.hpp"
#include "nav_2d_msgs/msg/path2_d.hpp"
#include "nav_2d_msgs/msg/pose2_d_stamped.hpp"
#include "nav_msgs/msg/path.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/convert.h"
namespace nav_2d_utils
{
51 geometry_msgs::msg::Twist twist2Dto3D( const nav_2d_msgs::msg::Twist2D & cmd_vel_2d );
52 nav_2d_msgs::msg::Twist2D twist3Dto2D( const geometry_msgs::msg::Twist & cmd_vel );
// nav_2d_msgs::msg::Pose2DStamped stampedPoseToPose2D( const tf2::Stamped<tf2::Pose>& pose );
54 nav_2d_msgs::msg::Pose2DStamped poseStampedToPose2D( const geometry_msgs::msg::PoseStamped & pose );
55 geometry_msgs::msg::Pose2D poseToPose2D( const geometry_msgs::msg::Pose & pose );
56 geometry_msgs::msg::Pose pose2DToPose( const geometry_msgs::msg::Pose2D & pose2d );
57 geometry_msgs::msg::PoseStamped pose2DToPoseStamped(
const nav_2d_msgs::msg::Pose2DStamped & pose2d );
59 geometry_msgs::msg::PoseStamped pose2DToPoseStamped(
const geometry_msgs::msg::Pose2D & pose2d,
const std::string & frame, const rclcpp::Time & stamp );
62 nav_msgs::msg::Path posesToPath( const std::vector<geometry_msgs::msg::PoseStamped> & poses );
63 nav_2d_msgs::msg::Path2D pathToPath2D( const nav_msgs::msg::Path & path );
64 nav_msgs::msg::Path poses2DToPath(
const std::vector<geometry_msgs::msg::Pose2D> & poses,
const std::string & frame, const rclcpp::Time & stamp );
67 nav_msgs::msg::Path pathToPath( const nav_2d_msgs::msg::Path2D & path2d );
} // namespace nav_2d_utils
#endif // NAV_2D_UTILS__CONVERSIONS_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef NAV_2D_UTILS__ODOM_SUBSCRIBER_HPP_
#define NAV_2D_UTILS__ODOM_SUBSCRIBER_HPP_
#include <cmath>
#include <memory>
#include <mutex>
#include <string>
#include "nav_2d_msgs/msg/twist2_d_stamped.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "rclcpp/rclcpp.hpp"
#include "nav2_util/node_utils.hpp"
namespace nav_2d_utils
{
/**
* @class OdomSubscriber
* Wrapper for some common odometry operations. Subscribes to the topic with a mutex.
*/
55 class OdomSubscriber
{
public:
/**
* @brief Constructor that subscribes to an Odometry topic
*
* @param nh NodeHandle for creating subscriber
* @param default_topic Name of the topic that will be loaded of the odom_topic param is not set.
*/
64 explicit OdomSubscriber(
65 nav2_util::LifecycleNode::SharedPtr nh,
66 std::string default_topic = "odom" )
{
nav2_util::declare_parameter_if_not_declared(
nh, "odom_topic", rclcpp::ParameterValue( default_topic ) );
std::string odom_topic;
nh->get_parameter_or( "odom_topic", odom_topic, default_topic );
odom_sub_ =
nh->create_subscription<nav_msgs::msg::Odometry>(
odom_topic,
rclcpp::SystemDefaultsQoS( ),
std::bind( &OdomSubscriber::odomCallback, this, std::placeholders::_1 ) );
}
80 inline nav_2d_msgs::msg::Twist2D getTwist( ) {return odom_vel_.velocity;}
81 inline nav_2d_msgs::msg::Twist2DStamped getTwistStamped( ) {return odom_vel_;}
protected:
84 void odomCallback( const nav_msgs::msg::Odometry::SharedPtr msg )
{
// ROS_INFO_ONCE( "odom received!" );
std::lock_guard<std::mutex> lock( odom_mutex_ );
odom_vel_.header = msg->header;
odom_vel_.velocity.x = msg->twist.twist.linear.x;
odom_vel_.velocity.y = msg->twist.twist.linear.y;
odom_vel_.velocity.theta = msg->twist.twist.angular.z;
}
94 rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
95 nav_2d_msgs::msg::Twist2DStamped odom_vel_;
96 std::mutex odom_mutex_;
};
} // namespace nav_2d_utils
#endif // NAV_2D_UTILS__ODOM_SUBSCRIBER_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef NAV_2D_UTILS__PARAMETERS_HPP_
#define NAV_2D_UTILS__PARAMETERS_HPP_
#include <string>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/node_utils.hpp"
// TODO( crdelsey ): Remove when code is re-enabled
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
namespace nav_2d_utils
{
/**
* @brief Search for a parameter and load it, or use the default value
*
* This templated function shortens a commonly used ROS pattern in which you
* search for a parameter and get its value if it exists, otherwise returning a default value.
*
* @param nh NodeHandle to start the parameter search from
* @param param_name Name of the parameter to search for
* @param default_value Value to return if not found
* @return Value of parameter if found, otherwise the default_value
*/
template<class param_t>
63 param_t searchAndGetParam(
const nav2_util::LifecycleNode::SharedPtr & nh, const std::string & param_name,
const param_t & default_value )
{
param_t value;
nav2_util::declare_parameter_if_not_declared(
nh, param_name,
rclcpp::ParameterValue( default_value ) );
nh->get_parameter( param_name, value );
return value;
}
} // namespace nav_2d_utils
#pragma GCC diagnostic pop
#endif // NAV_2D_UTILS__PARAMETERS_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef NAV_2D_UTILS__PATH_OPS_HPP_
#define NAV_2D_UTILS__PATH_OPS_HPP_
#include "nav_2d_msgs/msg/path2_d.hpp"
namespace nav_2d_utils
{
/**
* @brief Increase plan resolution to match that of the costmap by adding points linearly between points
*
* @param global_plan_in input plan
* @param resolution desired distance between waypoints
* @return Higher resolution plan
*/
49 nav_2d_msgs::msg::Path2D adjustPlanResolution(
const nav_2d_msgs::msg::Path2D & global_plan_in,
double resolution );
} // namespace nav_2d_utils
#endif // NAV_2D_UTILS__PATH_OPS_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef NAV_2D_UTILS__TF_HELP_HPP_
#define NAV_2D_UTILS__TF_HELP_HPP_
#include <string>
#include <memory>
#include "tf2_ros/buffer.h"
#include "nav_2d_utils/conversions.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav_2d_msgs/msg/pose2_d_stamped.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
namespace nav_2d_utils
{
/**
* @brief Transform a PoseStamped from one frame to another while catching exceptions
*
* Also returns immediately if the frames are equal.
* @param tf Smart pointer to TFListener
* @param frame Frame to transform the pose into
* @param in_pose Pose to transform
* @param out_pose Place to store the resulting transformed pose
* @return True if successful transform
*/
58 bool transformPose(
const std::shared_ptr<tf2_ros::Buffer> tf,
const std::string frame,
const geometry_msgs::msg::PoseStamped & in_pose,
geometry_msgs::msg::PoseStamped & out_pose,
rclcpp::Duration & transform_tolerance
);
/**
* @brief Transform a Pose2DStamped from one frame to another while catching exceptions
*
* Also returns immediately if the frames are equal.
* @param tf Smart pointer to TFListener
* @param frame Frame to transform the pose into
* @param in_pose Pose to transform
* @param out_pose Place to store the resulting transformed pose
* @return True if successful transform
*/
76 bool transformPose(
const std::shared_ptr<tf2_ros::Buffer> tf,
const std::string frame,
const nav_2d_msgs::msg::Pose2DStamped & in_pose,
nav_2d_msgs::msg::Pose2DStamped & out_pose,
rclcpp::Duration & transform_tolerance
);
} // namespace nav_2d_utils
#endif // NAV_2D_UTILS__TF_HELP_HPP_
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "nav_2d_utils/conversions.hpp"
#include <vector>
#include <string>
#include "geometry_msgs/msg/pose.hpp"
#include "geometry_msgs/msg/pose2_d.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpedantic"
#include "tf2/utils.h"
#pragma GCC diagnostic pop
#include "nav2_util/geometry_utils.hpp"
namespace nav_2d_utils
{
using nav2_util::geometry_utils::orientationAroundZAxis;
56 geometry_msgs::msg::Twist twist2Dto3D( const nav_2d_msgs::msg::Twist2D & cmd_vel_2d )
{
geometry_msgs::msg::Twist cmd_vel;
cmd_vel.linear.x = cmd_vel_2d.x;
cmd_vel.linear.y = cmd_vel_2d.y;
cmd_vel.angular.z = cmd_vel_2d.theta;
return cmd_vel;
}
65 nav_2d_msgs::msg::Twist2D twist3Dto2D( const geometry_msgs::msg::Twist & cmd_vel )
{
nav_2d_msgs::msg::Twist2D cmd_vel_2d;
cmd_vel_2d.x = cmd_vel.linear.x;
cmd_vel_2d.y = cmd_vel.linear.y;
cmd_vel_2d.theta = cmd_vel.angular.z;
return cmd_vel_2d;
}
// nav_2d_msgs::msg::Pose2DStamped stampedPoseToPose2D( const tf2::Stamped<tf2::Pose>& pose )
// {
// nav_2d_msgs::msg::Pose2DStamped pose2d;
// pose2d.header.stamp = pose.stamp_;
// pose2d.header.frame_id = pose.frame_id_;
// pose2d.pose.x = pose.getOrigin( ).getX( );
// pose2d.pose.y = pose.getOrigin( ).getY( );
// pose2d.pose.theta = tf::getYaw( pose.getRotation( ) );
// return pose2d;
// }
85 nav_2d_msgs::msg::Pose2DStamped poseStampedToPose2D( const geometry_msgs::msg::PoseStamped & pose )
{
nav_2d_msgs::msg::Pose2DStamped pose2d;
pose2d.header = pose.header;
pose2d.pose.x = pose.pose.position.x;
pose2d.pose.y = pose.pose.position.y;
pose2d.pose.theta = tf2::getYaw( pose.pose.orientation );
return pose2d;
}
95 geometry_msgs::msg::Pose2D poseToPose2D( const geometry_msgs::msg::Pose & pose )
{
geometry_msgs::msg::Pose2D pose2d;
pose2d.x = pose.position.x;
pose2d.y = pose.position.y;
pose2d.theta = tf2::getYaw( pose.orientation );
return pose2d;
}
104 geometry_msgs::msg::Pose pose2DToPose( const geometry_msgs::msg::Pose2D & pose2d )
{
geometry_msgs::msg::Pose pose;
pose.position.x = pose2d.x;
pose.position.y = pose2d.y;
pose.orientation = orientationAroundZAxis( pose2d.theta );
return pose;
}
113 geometry_msgs::msg::PoseStamped pose2DToPoseStamped(
const nav_2d_msgs::msg::Pose2DStamped & pose2d )
{
geometry_msgs::msg::PoseStamped pose;
pose.header = pose2d.header;
pose.pose = pose2DToPose( pose2d.pose );
return pose;
}
122 geometry_msgs::msg::PoseStamped pose2DToPoseStamped(
const geometry_msgs::msg::Pose2D & pose2d,
const std::string & frame, const rclcpp::Time & stamp )
{
geometry_msgs::msg::PoseStamped pose;
pose.header.frame_id = frame;
pose.header.stamp = stamp;
pose.pose.position.x = pose2d.x;
pose.pose.position.y = pose2d.y;
pose.pose.orientation = orientationAroundZAxis( pose2d.theta );
return pose;
}
135 nav_msgs::msg::Path posesToPath( const std::vector<geometry_msgs::msg::PoseStamped> & poses )
{
nav_msgs::msg::Path path;
if ( poses.empty( ) ) {
return path;
}
path.poses.resize( poses.size( ) );
path.header.frame_id = poses[0].header.frame_id;
path.header.stamp = poses[0].header.stamp;
for ( unsigned int i = 0; i < poses.size( ); i++ ) {
path.poses[i] = poses[i];
}
return path;
}
150 nav_2d_msgs::msg::Path2D pathToPath2D( const nav_msgs::msg::Path & path )
{
nav_2d_msgs::msg::Path2D path2d;
path2d.header = path.header;
for ( auto & pose : path.poses ) {
path2d.poses.push_back( poseToPose2D( pose.pose ) );
}
return path2d;
}
161 nav_msgs::msg::Path poses2DToPath(
const std::vector<geometry_msgs::msg::Pose2D> & poses,
const std::string & frame, const rclcpp::Time & stamp )
{
nav_msgs::msg::Path path;
path.poses.resize( poses.size( ) );
path.header.frame_id = frame;
path.header.stamp = stamp;
for ( unsigned int i = 0; i < poses.size( ); i++ ) {
path.poses[i] = pose2DToPoseStamped( poses[i], frame, stamp );
}
return path;
}
175 nav_msgs::msg::Path pathToPath( const nav_2d_msgs::msg::Path2D & path2d )
{
nav_msgs::msg::Path path;
path.header = path2d.header;
path.poses.resize( path2d.poses.size( ) );
for ( unsigned int i = 0; i < path.poses.size( ); i++ ) {
path.poses[i].header = path2d.header;
path.poses[i].pose = pose2DToPose( path2d.poses[i] );
}
return path;
}
} // namespace nav_2d_utils
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "nav_2d_utils/path_ops.hpp"
#include <cmath>
using std::sqrt;
namespace nav_2d_utils
{
42 nav_2d_msgs::msg::Path2D adjustPlanResolution(
const nav_2d_msgs::msg::Path2D & global_plan_in,
double resolution )
{
nav_2d_msgs::msg::Path2D global_plan_out;
if ( global_plan_in.poses.size( ) == 0 ) {
return global_plan_out;
}
geometry_msgs::msg::Pose2D last = global_plan_in.poses[0];
global_plan_out.poses.push_back( last );
// we can take "holes" in the plan smaller than 2 grid cells ( squared = 4 )
double min_sq_resolution = resolution * resolution * 4.0;
for ( unsigned int i = 1; i < global_plan_in.poses.size( ); ++i ) {
geometry_msgs::msg::Pose2D loop = global_plan_in.poses[i];
double sq_dist = ( loop.x - last.x ) * ( loop.x - last.x ) + ( loop.y - last.y ) * ( loop.y - last.y );
if ( sq_dist > min_sq_resolution ) {
// add points in-between
double diff = sqrt( sq_dist ) - sqrt( min_sq_resolution );
int steps = static_cast<int>( diff / resolution ) - 1;
double steps_double = static_cast<double>( steps );
double delta_x = ( loop.x - last.x ) / steps_double;
double delta_y = ( loop.y - last.y ) / steps_double;
double delta_t = ( loop.theta - last.theta ) / steps_double;
for ( int j = 1; j < steps; ++j ) {
geometry_msgs::msg::Pose2D pose;
pose.x = last.x + j * delta_x;
pose.y = last.y + j * delta_y;
pose.theta = last.theta + j * delta_t;
global_plan_out.poses.push_back( pose );
}
}
global_plan_out.poses.push_back( global_plan_in.poses[i] );
last.x = loop.x;
last.y = loop.y;
}
return global_plan_out;
}
} // namespace nav_2d_utils
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2017, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <memory>
#include <string>
#include "nav_2d_utils/tf_help.hpp"
namespace nav_2d_utils
{
42 bool transformPose(
const std::shared_ptr<tf2_ros::Buffer> tf,
const std::string frame,
const geometry_msgs::msg::PoseStamped & in_pose,
geometry_msgs::msg::PoseStamped & out_pose,
rclcpp::Duration & transform_tolerance
)
{
if ( in_pose.header.frame_id == frame ) {
out_pose = in_pose;
return true;
}
try {
tf->transform( in_pose, out_pose, frame );
return true;
} catch ( tf2::ExtrapolationException & ex ) {
auto transform = tf->lookupTransform(
frame,
in_pose.header.frame_id,
tf2::TimePointZero
);
if (
( rclcpp::Time( in_pose.header.stamp ) - rclcpp::Time( transform.header.stamp ) ) >
transform_tolerance )
{
RCLCPP_ERROR(
rclcpp::get_logger( "tf_help" ),
"Transform data too old when converting from %s to %s",
in_pose.header.frame_id.c_str( ),
frame.c_str( )
);
RCLCPP_ERROR(
rclcpp::get_logger( "tf_help" ),
"Data time: %ds %uns, Transform time: %ds %uns",
in_pose.header.stamp.sec,
in_pose.header.stamp.nanosec,
transform.header.stamp.sec,
transform.header.stamp.nanosec
);
return false;
} else {
tf2::doTransform( in_pose, out_pose, transform );
return true;
}
} catch ( tf2::TransformException & ex ) {
RCLCPP_ERROR(
rclcpp::get_logger( "tf_help" ),
"Exception in transformPose: %s",
ex.what( )
);
return false;
}
return false;
}
98 bool transformPose(
const std::shared_ptr<tf2_ros::Buffer> tf,
const std::string frame,
const nav_2d_msgs::msg::Pose2DStamped & in_pose,
nav_2d_msgs::msg::Pose2DStamped & out_pose,
rclcpp::Duration & transform_tolerance
)
{
geometry_msgs::msg::PoseStamped in_3d_pose = pose2DToPoseStamped( in_pose );
geometry_msgs::msg::PoseStamped out_3d_pose;
bool ret = transformPose( tf, frame, in_3d_pose, out_3d_pose, transform_tolerance );
if ( ret ) {
out_pose = poseStampedToPose2D( out_3d_pose );
}
return ret;
}
} // namespace nav_2d_utils
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2018, Wilco Bonestroo
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <math.h>
#include <tf2/LinearMath/Quaternion.h>
#include <vector>
#include "gtest/gtest.h"
#include "nav_2d_utils/conversions.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/time.hpp"
using nav_2d_utils::posesToPath;
using nav_2d_utils::pathToPath;
47 TEST( nav_2d_utils, PosesToPathEmpty )
{
std::vector<geometry_msgs::msg::PoseStamped> poses;
nav_msgs::msg::Path path = posesToPath( poses );
EXPECT_EQ( path.poses.size( ), 0ul );
}
55 TEST( nav_2d_utils, PosesToPathNonEmpty )
{
std::vector<geometry_msgs::msg::PoseStamped> poses;
geometry_msgs::msg::PoseStamped pose1;
rclcpp::Time time1, time2;
auto node = rclcpp::Node::make_shared( "twod_utils_test_node" );
time1 = node->now( );
tf2::Quaternion quat1, quat2;
quat1.setRPY( 0, 0, 0.123 );
pose1.pose.position.x = 1.0;
pose1.pose.position.y = 2.0;
pose1.pose.orientation.w = quat1.w( );
pose1.pose.orientation.x = quat1.x( );
pose1.pose.orientation.y = quat1.y( );
pose1.pose.orientation.z = quat1.z( );
pose1.header.stamp = time1;
pose1.header.frame_id = "frame1_id";
geometry_msgs::msg::PoseStamped pose2;
pose2.pose.position.x = 4.0;
pose2.pose.position.y = 5.0;
quat2.setRPY( 0, 0, 0.987 );
pose2.pose.orientation.w = quat2.w( );
pose2.pose.orientation.x = quat2.x( );
pose2.pose.orientation.y = quat2.y( );
pose2.pose.orientation.z = quat2.z( );
time2 = node->now( );
pose2.header.stamp = time2;
pose2.header.frame_id = "frame2_id";
poses.push_back( pose1 );
poses.push_back( pose2 );
nav_msgs::msg::Path path = posesToPath( poses );
EXPECT_EQ( path.poses.size( ), 2ul );
EXPECT_EQ( path.poses[0].pose.position.x, 1.0 );
EXPECT_EQ( path.poses[0].pose.position.y, 2.0 );
EXPECT_EQ( path.poses[0].header.stamp, time1 );
EXPECT_EQ( path.poses[0].header.frame_id, "frame1_id" );
EXPECT_EQ( path.poses[1].pose.position.x, 4.0 );
EXPECT_EQ( path.poses[1].pose.position.y, 5.0 );
EXPECT_EQ( path.poses[1].header.frame_id, "frame2_id" );
EXPECT_EQ( path.header.stamp, time1 );
}
104 TEST( nav_2d_utils, PathToPathEmpty )
{
nav_2d_msgs::msg::Path2D path2d;
nav_msgs::msg::Path path = pathToPath( path2d );
EXPECT_EQ( path.poses.size( ), 0ul );
}
111 TEST( nav_2d_utils, PathToPathNoNEmpty )
{
nav_2d_msgs::msg::Path2D path2d;
geometry_msgs::msg::Pose2D pose1;
pose1.x = 1.0;
pose1.y = 2.0;
pose1.theta = M_PI / 2.0;
geometry_msgs::msg::Pose2D pose2;
pose2.x = 4.0;
pose2.y = 5.0;
pose2.theta = M_PI;
path2d.poses.push_back( pose1 );
path2d.poses.push_back( pose2 );
nav_msgs::msg::Path path = pathToPath( path2d );
EXPECT_EQ( path.poses.size( ), 2ul );
EXPECT_EQ( path.poses[0].pose.position.x, 1.0 );
EXPECT_EQ( path.poses[0].pose.position.y, 2.0 );
tf2::Quaternion quat;
quat.setRPY( 0, 0, M_PI / 2.0 );
EXPECT_EQ( path.poses[0].pose.orientation.w, quat.w( ) );
EXPECT_EQ( path.poses[0].pose.orientation.x, quat.x( ) );
EXPECT_EQ( path.poses[0].pose.orientation.y, quat.x( ) );
EXPECT_EQ( path.poses[0].pose.orientation.z, quat.z( ) );
EXPECT_EQ( path.poses[1].pose.position.x, 4.0 );
EXPECT_EQ( path.poses[1].pose.position.y, 5.0 );
quat.setRPY( 0, 0, M_PI );
EXPECT_EQ( path.poses[1].pose.orientation.w, quat.w( ) );
EXPECT_EQ( path.poses[1].pose.orientation.x, quat.x( ) );
EXPECT_EQ( path.poses[1].pose.orientation.y, quat.x( ) );
EXPECT_EQ( path.poses[1].pose.orientation.z, quat.z( ) );
}
149 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2018, Wilco Bonestroo
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <cmath>
#include "gtest/gtest.h"
#include "nav_2d_utils/path_ops.hpp"
using std::sqrt;
using nav_2d_utils::adjustPlanResolution;
42 TEST( path_ops_test, AdjustResolutionEmpty )
{
nav_2d_msgs::msg::Path2D in;
nav_2d_msgs::msg::Path2D out = adjustPlanResolution( in, 2.0 );
EXPECT_EQ( out.poses.size( ), 0ul );
}
49 TEST( path_ops_test, AdjustResolutionSimple )
{
nav_2d_msgs::msg::Path2D in;
const float RESOLUTION = 20.0;
geometry_msgs::msg::Pose2D pose1;
pose1.x = 0.0;
pose1.y = 0.0;
geometry_msgs::msg::Pose2D pose2;
pose2.x = 100.0;
pose2.y = 0.0;
in.poses.push_back( pose1 );
in.poses.push_back( pose2 );
nav_2d_msgs::msg::Path2D out = adjustPlanResolution( in, RESOLUTION );
float length = 100;
ulong number_of_points = ceil( length / ( 2 * RESOLUTION ) );
EXPECT_EQ( out.poses.size( ), number_of_points );
float max_length = length / ( number_of_points - 1 );
for ( unsigned int i = 1; i < out.poses.size( ); i++ ) {
pose1 = out.poses[i - 1];
pose2 = out.poses[i];
double sq_dist = ( pose1.x - pose2.x ) * ( pose1.x - pose2.x ) +
( pose1.y - pose2.y ) * ( pose1.y - pose2.y );
EXPECT_TRUE( sqrt( sq_dist ) <= max_length );
}
}
1 /*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2018, Wilco Bonestroo
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <memory>
#include <string>
#include "gtest/gtest.h"
#include "nav_2d_utils/tf_help.hpp"
41 TEST( TF_Help, TransformToSelf ) {
bool result;
std::shared_ptr<tf2_ros::Buffer> tf;
std::string frame = "frame_id";
geometry_msgs::msg::PoseStamped in_pose;
in_pose.header.frame_id = "frame_id";
in_pose.pose.position.x = 1.0;
in_pose.pose.position.y = 2.0;
in_pose.pose.position.z = 3.0;
tf2::Quaternion qt;
qt.setRPY( 0.5, 1.0, 1.5 );
in_pose.pose.orientation.w = qt.w( );
in_pose.pose.orientation.x = qt.x( );
in_pose.pose.orientation.y = qt.y( );
in_pose.pose.orientation.z = qt.z( );
geometry_msgs::msg::PoseStamped out_pose;
rclcpp::Duration transform_tolerance( 0, 500 );
result = nav_2d_utils::transformPose( tf, frame, in_pose, out_pose, transform_tolerance );
EXPECT_TRUE( result );
EXPECT_EQ( out_pose.header.frame_id, "frame_id" );
EXPECT_EQ( out_pose.pose.position.x, 1.0 );
EXPECT_EQ( out_pose.pose.position.y, 2.0 );
EXPECT_EQ( out_pose.pose.position.z, 3.0 );
EXPECT_EQ( out_pose.pose.orientation.w, qt.w( ) );
EXPECT_EQ( out_pose.pose.orientation.x, qt.x( ) );
EXPECT_EQ( out_pose.pose.orientation.y, qt.y( ) );
EXPECT_EQ( out_pose.pose.orientation.z, qt.z( ) );
}
74 TEST( TF_Help, EmptyBuffer ) {
auto clock = std::make_shared<rclcpp::Clock>( RCL_ROS_TIME );
auto buffer = std::make_shared<tf2_ros::Buffer>( clock );
std::string frame = "frame_id";
geometry_msgs::msg::PoseStamped in_pose;
in_pose.header.frame_id = "other_frame_id";
in_pose.pose.position.x = 1.0;
in_pose.pose.position.y = 2.0;
in_pose.pose.position.z = 3.0;
tf2::Quaternion qt;
qt.setRPY( 0.5, 1.0, 1.5 );
in_pose.pose.orientation.w = qt.w( );
in_pose.pose.orientation.x = qt.x( );
in_pose.pose.orientation.y = qt.y( );
in_pose.pose.orientation.z = qt.z( );
geometry_msgs::msg::PoseStamped out_pose;
rclcpp::Duration transform_tolerance( 0, 500 );
bool result;
result = nav_2d_utils::transformPose( buffer, frame, in_pose, out_pose, transform_tolerance );
EXPECT_FALSE( result );
}
// Copyright ( c ) 2019 Intel Corporation
// Copyright ( c ) 2022 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_LIFECYCLE_MANAGER__LIFECYCLE_MANAGER_HPP_
#define NAV2_LIFECYCLE_MANAGER__LIFECYCLE_MANAGER_HPP_
#include <map>
#include <memory>
#include <string>
#include <thread>
#include <unordered_map>
#include <vector>
#include "nav2_util/lifecycle_service_client.hpp"
#include "nav2_util/node_thread.hpp"
#include "rclcpp/rclcpp.hpp"
#include "std_srvs/srv/empty.hpp"
#include "nav2_msgs/srv/manage_lifecycle_nodes.hpp"
#include "std_srvs/srv/trigger.hpp"
#include "bondcpp/bond.hpp"
#include "diagnostic_updater/diagnostic_updater.hpp"
namespace nav2_lifecycle_manager
{
using namespace std::chrono_literals; // NOLINT
using nav2_msgs::srv::ManageLifecycleNodes;
/**
* @class nav2_lifecycle_manager::LifecycleManager
* @brief Implements service interface to transition the lifecycle nodes of
* Nav2 stack. It receives transition request and then uses lifecycle
* interface to change lifecycle node's state.
*/
47 class LifecycleManager : public rclcpp::Node
{
public:
/**
* @brief A constructor for nav2_lifecycle_manager::LifecycleManager
* @param options Additional options to control creation of the node.
*/
54 explicit LifecycleManager( const rclcpp::NodeOptions & options = rclcpp::NodeOptions( ) );
/**
* @brief A destructor for nav2_lifecycle_manager::LifecycleManager
*/
58 ~LifecycleManager( );
protected:
// Callback group used by services and timers
62 rclcpp::CallbackGroup::SharedPtr callback_group_;
63 std::unique_ptr<nav2_util::NodeThread> service_thread_;
// The services provided by this node
66 rclcpp::Service<ManageLifecycleNodes>::SharedPtr manager_srv_;
67 rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr is_active_srv_;
/**
* @brief Lifecycle node manager callback function
* @param request_header Header of the service request
* @param request Service request
* @param reponse Service response
*/
74 void managerCallback(
75 const std::shared_ptr<rmw_request_id_t> request_header,
76 const std::shared_ptr<ManageLifecycleNodes::Request> request,
77 std::shared_ptr<ManageLifecycleNodes::Response> response );
/**
* @brief Trigger callback function checks if the managed nodes are in active
* state.
* @param request_header Header of the request
* @param request Service request
* @param reponse Service response
*/
85 void isActiveCallback(
86 const std::shared_ptr<rmw_request_id_t> request_header,
87 const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
88 std::shared_ptr<std_srvs::srv::Trigger::Response> response );
// Support functions for the service calls
/**
* @brief Start up managed nodes.
* @return true or false
*/
95 bool startup( );
/**
* @brief Deactivate, clean up and shut down all the managed nodes.
* @return true or false
*/
100 bool shutdown( );
/**
* @brief Reset all the managed nodes.
* @return true or false
*/
105 bool reset( bool hard_reset = false );
/**
* @brief Pause all the managed nodes.
* @return true or false
*/
110 bool pause( );
/**
* @brief Resume all the managed nodes.
* @return true or false
*/
115 bool resume( );
// Support function for creating service clients
/**
* @brief Support function for creating service clients
*/
121 void createLifecycleServiceClients( );
// Support functions for shutdown
/**
* @brief Support function for shutdown
*/
127 void shutdownAllNodes( );
/**
* @brief Destroy all the lifecycle service clients.
*/
131 void destroyLifecycleServiceClients( );
// Support function for creating bond timer
/**
* @brief Support function for creating bond timer
*/
137 void createBondTimer( );
// Support function for creating bond connection
/**
* @brief Support function for creating bond connections
*/
143 bool createBondConnection( const std::string & node_name );
// Support function for killing bond connections
/**
* @brief Support function for killing bond connections
*/
149 void destroyBondTimer( );
// Support function for checking on bond connections
/**
* @ brief Support function for checking on bond connections
* will take down system if there's something non-responsive
*/
156 void checkBondConnections( );
// Support function for checking if bond connections come back after respawn
/**
* @ brief Support function for checking on bond connections
* will bring back the system if something goes from non-responsive to responsive
*/
163 void checkBondRespawnConnection( );
/**
* @brief For a node, transition to the new target state
*/
168 bool changeStateForNode(
169 const std::string & node_name,
170 std::uint8_t transition );
/**
* @brief For each node in the map, transition to the new target state
*/
175 bool changeStateForAllNodes( std::uint8_t transition, bool hard_change = false );
// Convenience function to highlight the output on the console
/**
* @brief Helper function to highlight the output on the console
*/
181 void message( const std::string & msg );
// Diagnostics functions
/**
* @brief function to check if the Nav2 system is active
*/
187 void CreateActiveDiagnostic( diagnostic_updater::DiagnosticStatusWrapper & stat );
// Timer thread to look at bond connections
190 rclcpp::TimerBase::SharedPtr init_timer_;
191 rclcpp::TimerBase::SharedPtr bond_timer_;
192 rclcpp::TimerBase::SharedPtr bond_respawn_timer_;
193 std::chrono::milliseconds bond_timeout_;
// A map of all nodes to check bond connection
std::map<std::string, std::shared_ptr<bond::Bond>> bond_map_;
// A map of all nodes to be controlled
std::map<std::string, std::shared_ptr<nav2_util::LifecycleServiceClient>> node_map_;
std::map<std::uint8_t, std::string> transition_label_map_;
// A map of the expected transitions to primary states
std::unordered_map<std::uint8_t, std::uint8_t> transition_state_map_;
// The names of the nodes to be managed, in the order of desired bring-up
std::vector<std::string> node_names_;
// Whether to automatically start up the system
bool autostart_;
bool attempt_respawn_reconnection_;
bool system_active_{false};
diagnostic_updater::Updater diagnostics_updater_;
rclcpp::Time bond_respawn_start_time_{0};
217 rclcpp::Duration bond_respawn_max_duration_{10s};
};
} // namespace nav2_lifecycle_manager
#endif // NAV2_LIFECYCLE_MANAGER__LIFECYCLE_MANAGER_HPP_
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_LIFECYCLE_MANAGER__LIFECYCLE_MANAGER_CLIENT_HPP_
#define NAV2_LIFECYCLE_MANAGER__LIFECYCLE_MANAGER_CLIENT_HPP_
#include <memory>
#include <string>
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "std_srvs/srv/empty.hpp"
#include "nav2_msgs/srv/manage_lifecycle_nodes.hpp"
#include "std_srvs/srv/trigger.hpp"
#include "nav2_util/service_client.hpp"
namespace nav2_lifecycle_manager
{
/**
* @enum nav2_lifecycle_manager::SystemStatus
* @brief Enum class representing the status of the system.
*/
37 enum class SystemStatus {ACTIVE, INACTIVE, TIMEOUT};
/**
* @class nav2_lifecycle_manager::LifeCycleMangerClient
* @brief The LifecycleManagerClient sends requests to the LifecycleManager to
* control the lifecycle state of the navigation modules.
*/
43 class LifecycleManagerClient
{
public:
/**
* @brief A constructor for LifeCycleMangerClient
* @param name Managed node name
* @param parent_node Node that execute the service calls
*/
51 explicit LifecycleManagerClient(
52 const std::string & name,
53 std::shared_ptr<rclcpp::Node> parent_node );
// Client-side interface to the Nav2 lifecycle manager
/**
* @brief Make start up service call
* @return true or false
*/
60 bool startup( const std::chrono::nanoseconds timeout = std::chrono::nanoseconds( -1 ) );
/**
* @brief Make shutdown service call
* @return true or false
*/
65 bool shutdown( const std::chrono::nanoseconds timeout = std::chrono::nanoseconds( -1 ) );
/**
* @brief Make pause service call
* @return true or false
*/
70 bool pause( const std::chrono::nanoseconds timeout = std::chrono::nanoseconds( -1 ) );
/**
* @brief Make resume service call
* @return true or false
*/
75 bool resume( const std::chrono::nanoseconds timeout = std::chrono::nanoseconds( -1 ) );
/**
* @brief Make reset service call
* @return true or false
*/
80 bool reset( const std::chrono::nanoseconds timeout = std::chrono::nanoseconds( -1 ) );
/**
* @brief Check if lifecycle node manager server is active
* @return ACTIVE or INACTIVE or TIMEOUT
*/
85 SystemStatus is_active( const std::chrono::nanoseconds timeout = std::chrono::nanoseconds( -1 ) );
// A couple convenience methods to facilitate scripting tests
/**
* @brief Set initial pose with covariance
* @param x X position
* @param y Y position
* @param theta orientation
*/
94 void set_initial_pose( double x, double y, double theta );
/**
* @brief Send goal pose to NavigationToPose action server
* @param x X position
* @param y Y position
* @param theta orientation
* @return true or false
*/
102 bool navigate_to_pose( double x, double y, double theta );
protected:
using ManageLifecycleNodes = nav2_msgs::srv::ManageLifecycleNodes;
/**
* @brief A generic method used to call startup, shutdown, etc.
* @param command
*/
bool callService(
uint8_t command,
const std::chrono::nanoseconds timeout = std::chrono::nanoseconds( -1 ) );
// The node to use for the service call
rclcpp::Node::SharedPtr node_;
std::shared_ptr<nav2_util::ServiceClient<ManageLifecycleNodes>> manager_client_;
std::shared_ptr<nav2_util::ServiceClient<std_srvs::srv::Trigger>> is_active_client_;
std::string manage_service_name_;
std::string active_service_name_;
};
} // namespace nav2_lifecycle_manager
#endif // NAV2_LIFECYCLE_MANAGER__LIFECYCLE_MANAGER_CLIENT_HPP_
1 // Copyright ( c ) 2019 Intel Corporation
// Copyright ( c ) 2022 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "nav2_lifecycle_manager/lifecycle_manager.hpp"
#include <chrono>
#include <memory>
#include <string>
#include <vector>
#include "rclcpp/rclcpp.hpp"
using namespace std::chrono_literals;
using namespace std::placeholders;
using lifecycle_msgs::msg::Transition;
using lifecycle_msgs::msg::State;
using nav2_util::LifecycleServiceClient;
namespace nav2_lifecycle_manager
{
35 LifecycleManager::LifecycleManager( const rclcpp::NodeOptions & options )
: Node( "lifecycle_manager", options ), diagnostics_updater_( this )
{
RCLCPP_INFO( get_logger( ), "Creating" );
// The list of names is parameterized, allowing this module to be used with a different set
// of nodes
declare_parameter( "node_names", rclcpp::PARAMETER_STRING_ARRAY );
declare_parameter( "autostart", rclcpp::ParameterValue( false ) );
declare_parameter( "bond_timeout", 4.0 );
declare_parameter( "bond_respawn_max_duration", 10.0 );
declare_parameter( "attempt_respawn_reconnection", true );
node_names_ = get_parameter( "node_names" ).as_string_array( );
get_parameter( "autostart", autostart_ );
double bond_timeout_s;
get_parameter( "bond_timeout", bond_timeout_s );
bond_timeout_ = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::duration<double>( bond_timeout_s ) );
double respawn_timeout_s;
get_parameter( "bond_respawn_max_duration", respawn_timeout_s );
bond_respawn_max_duration_ = rclcpp::Duration::from_seconds( respawn_timeout_s );
get_parameter( "attempt_respawn_reconnection", attempt_respawn_reconnection_ );
callback_group_ = create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false );
manager_srv_ = create_service<ManageLifecycleNodes>(
get_name( ) + std::string( "/manage_nodes" ),
std::bind( &LifecycleManager::managerCallback, this, _1, _2, _3 ),
rmw_qos_profile_services_default,
callback_group_ );
is_active_srv_ = create_service<std_srvs::srv::Trigger>(
get_name( ) + std::string( "/is_active" ),
std::bind( &LifecycleManager::isActiveCallback, this, _1, _2, _3 ),
rmw_qos_profile_services_default,
callback_group_ );
transition_state_map_[Transition::TRANSITION_CONFIGURE] = State::PRIMARY_STATE_INACTIVE;
transition_state_map_[Transition::TRANSITION_CLEANUP] = State::PRIMARY_STATE_UNCONFIGURED;
transition_state_map_[Transition::TRANSITION_ACTIVATE] = State::PRIMARY_STATE_ACTIVE;
transition_state_map_[Transition::TRANSITION_DEACTIVATE] = State::PRIMARY_STATE_INACTIVE;
transition_state_map_[Transition::TRANSITION_UNCONFIGURED_SHUTDOWN] =
State::PRIMARY_STATE_FINALIZED;
transition_label_map_[Transition::TRANSITION_CONFIGURE] = std::string( "Configuring " );
transition_label_map_[Transition::TRANSITION_CLEANUP] = std::string( "Cleaning up " );
transition_label_map_[Transition::TRANSITION_ACTIVATE] = std::string( "Activating " );
transition_label_map_[Transition::TRANSITION_DEACTIVATE] = std::string( "Deactivating " );
transition_label_map_[Transition::TRANSITION_UNCONFIGURED_SHUTDOWN] =
std::string( "Shutting down " );
init_timer_ = this->create_wall_timer(
0s,
[this]( ) -> void {
init_timer_->cancel( );
createLifecycleServiceClients( );
if ( autostart_ ) {
init_timer_ = this->create_wall_timer(
0s,
[this]( ) -> void {
init_timer_->cancel( );
startup( );
},
callback_group_ );
}
auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>( );
executor->add_callback_group( callback_group_, get_node_base_interface( ) );
service_thread_ = std::make_unique<nav2_util::NodeThread>( executor );
} );
diagnostics_updater_.setHardwareID( "Nav2" );
diagnostics_updater_.add( "Nav2 Health", this, &LifecycleManager::CreateActiveDiagnostic );
}
110 LifecycleManager::~LifecycleManager( )
{
RCLCPP_INFO( get_logger( ), "Destroying %s", get_name( ) );
service_thread_.reset( );
}
void
117 LifecycleManager::managerCallback(
118 const std::shared_ptr<rmw_request_id_t>/*request_header*/,
119 const std::shared_ptr<ManageLifecycleNodes::Request> request,
120 std::shared_ptr<ManageLifecycleNodes::Response> response )
{
switch ( request->command ) {
case ManageLifecycleNodes::Request::STARTUP:
response->success = startup( );
break;
case ManageLifecycleNodes::Request::RESET:
response->success = reset( );
break;
case ManageLifecycleNodes::Request::SHUTDOWN:
response->success = shutdown( );
break;
case ManageLifecycleNodes::Request::PAUSE:
response->success = pause( );
break;
case ManageLifecycleNodes::Request::RESUME:
response->success = resume( );
break;
}
}
void
142 LifecycleManager::isActiveCallback(
143 const std::shared_ptr<rmw_request_id_t>/*request_header*/,
144 const std::shared_ptr<std_srvs::srv::Trigger::Request>/*request*/,
145 std::shared_ptr<std_srvs::srv::Trigger::Response> response )
{
response->success = system_active_;
}
void
151 LifecycleManager::CreateActiveDiagnostic( diagnostic_updater::DiagnosticStatusWrapper & stat )
{
if ( system_active_ ) {
stat.summary( diagnostic_msgs::msg::DiagnosticStatus::OK, "Nav2 is active" );
} else {
stat.summary( diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Nav2 is inactive" );
}
}
void
161 LifecycleManager::createLifecycleServiceClients( )
{
message( "Creating and initializing lifecycle service clients" );
for ( auto & node_name : node_names_ ) {
node_map_[node_name] =
std::make_shared<LifecycleServiceClient>( node_name, shared_from_this( ) );
}
}
void
171 LifecycleManager::destroyLifecycleServiceClients( )
{
message( "Destroying lifecycle service clients" );
for ( auto & kv : node_map_ ) {
kv.second.reset( );
}
}
bool
180 LifecycleManager::createBondConnection( const std::string & node_name )
{
const double timeout_ns =
std::chrono::duration_cast<std::chrono::nanoseconds>( bond_timeout_ ).count( );
const double timeout_s = timeout_ns / 1e9;
if ( bond_map_.find( node_name ) == bond_map_.end( ) && bond_timeout_.count( ) > 0.0 ) {
bond_map_[node_name] =
std::make_shared<bond::Bond>( "bond", node_name, shared_from_this( ) );
bond_map_[node_name]->setHeartbeatTimeout( timeout_s );
bond_map_[node_name]->setHeartbeatPeriod( 0.10 );
bond_map_[node_name]->start( );
if (
!bond_map_[node_name]->waitUntilFormed(
rclcpp::Duration( rclcpp::Duration::from_nanoseconds( timeout_ns / 2 ) ) ) )
{
RCLCPP_ERROR(
get_logger( ),
"Server %s was unable to be reached after %0.2fs by bond. "
"This server may be misconfigured.",
node_name.c_str( ), timeout_s );
return false;
}
RCLCPP_INFO( get_logger( ), "Server %s connected with bond.", node_name.c_str( ) );
}
return true;
}
bool
210 LifecycleManager::changeStateForNode( const std::string & node_name, std::uint8_t transition )
{
message( transition_label_map_[transition] + node_name );
if ( !node_map_[node_name]->change_state( transition ) ||
!( node_map_[node_name]->get_state( ) == transition_state_map_[transition] ) )
{
RCLCPP_ERROR( get_logger( ), "Failed to change state for node: %s", node_name.c_str( ) );
return false;
}
if ( transition == Transition::TRANSITION_ACTIVATE ) {
return createBondConnection( node_name );
} else if ( transition == Transition::TRANSITION_DEACTIVATE ) {
bond_map_.erase( node_name );
}
return true;
}
bool
231 LifecycleManager::changeStateForAllNodes( std::uint8_t transition, bool hard_change )
{
// Hard change will continue even if a node fails
if ( transition == Transition::TRANSITION_CONFIGURE ||
transition == Transition::TRANSITION_ACTIVATE )
{
for ( auto & node_name : node_names_ ) {
try {
if ( !changeStateForNode( node_name, transition ) && !hard_change ) {
return false;
}
} catch ( const std::runtime_error & e ) {
RCLCPP_ERROR(
get_logger( ),
"Failed to change state for node: %s. Exception: %s.", node_name.c_str( ), e.what( ) );
return false;
}
}
} else {
std::vector<std::string>::reverse_iterator rit;
for ( rit = node_names_.rbegin( ); rit != node_names_.rend( ); ++rit ) {
try {
if ( !changeStateForNode( *rit, transition ) && !hard_change ) {
return false;
}
} catch ( const std::runtime_error & e ) {
RCLCPP_ERROR(
get_logger( ),
"Failed to change state for node: %s. Exception: %s.", ( *rit ).c_str( ), e.what( ) );
return false;
}
}
}
return true;
}
void
268 LifecycleManager::shutdownAllNodes( )
{
message( "Deactivate, cleanup, and shutdown nodes" );
changeStateForAllNodes( Transition::TRANSITION_DEACTIVATE );
changeStateForAllNodes( Transition::TRANSITION_CLEANUP );
changeStateForAllNodes( Transition::TRANSITION_UNCONFIGURED_SHUTDOWN );
}
bool
277 LifecycleManager::startup( )
{
message( "Starting managed nodes bringup..." );
if ( !changeStateForAllNodes( Transition::TRANSITION_CONFIGURE ) ||
!changeStateForAllNodes( Transition::TRANSITION_ACTIVATE ) )
{
RCLCPP_ERROR( get_logger( ), "Failed to bring up all requested nodes. Aborting bringup." );
return false;
}
message( "Managed nodes are active" );
system_active_ = true;
createBondTimer( );
return true;
}
bool
293 LifecycleManager::shutdown( )
{
system_active_ = false;
destroyBondTimer( );
message( "Shutting down managed nodes..." );
shutdownAllNodes( );
destroyLifecycleServiceClients( );
message( "Managed nodes have been shut down" );
return true;
}
bool
306 LifecycleManager::reset( bool hard_reset )
{
system_active_ = false;
destroyBondTimer( );
message( "Resetting managed nodes..." );
// Should transition in reverse order
if ( !changeStateForAllNodes( Transition::TRANSITION_DEACTIVATE, hard_reset ) ||
!changeStateForAllNodes( Transition::TRANSITION_CLEANUP, hard_reset ) )
{
if ( !hard_reset ) {
RCLCPP_ERROR( get_logger( ), "Failed to reset nodes: aborting reset" );
return false;
}
}
message( "Managed nodes have been reset" );
return true;
}
bool
327 LifecycleManager::pause( )
{
system_active_ = false;
destroyBondTimer( );
message( "Pausing managed nodes..." );
if ( !changeStateForAllNodes( Transition::TRANSITION_DEACTIVATE ) ) {
RCLCPP_ERROR( get_logger( ), "Failed to pause nodes: aborting pause" );
return false;
}
message( "Managed nodes have been paused" );
return true;
}
bool
343 LifecycleManager::resume( )
{
message( "Resuming managed nodes..." );
if ( !changeStateForAllNodes( Transition::TRANSITION_ACTIVATE ) ) {
RCLCPP_ERROR( get_logger( ), "Failed to resume nodes: aborting resume" );
return false;
}
message( "Managed nodes are active" );
system_active_ = true;
createBondTimer( );
return true;
}
void
358 LifecycleManager::createBondTimer( )
{
if ( bond_timeout_.count( ) <= 0 ) {
return;
}
message( "Creating bond timer..." );
bond_timer_ = this->create_wall_timer(
200ms,
std::bind( &LifecycleManager::checkBondConnections, this ),
callback_group_ );
}
void
372 LifecycleManager::destroyBondTimer( )
{
if ( bond_timer_ ) {
message( "Terminating bond timer..." );
bond_timer_->cancel( );
bond_timer_.reset( );
}
}
void
382 LifecycleManager::checkBondConnections( )
{
if ( !system_active_ || !rclcpp::ok( ) || bond_map_.empty( ) ) {
return;
}
for ( auto & node_name : node_names_ ) {
if ( !rclcpp::ok( ) ) {
return;
}
if ( bond_map_[node_name]->isBroken( ) ) {
message(
std::string(
"Have not received a heartbeat from " + node_name + "." ) );
// if one is down, bring them all down
RCLCPP_ERROR(
get_logger( ),
"CRITICAL FAILURE: SERVER %s IS DOWN after not receiving a heartbeat for %i ms."
" Shutting down related nodes.",
node_name.c_str( ), static_cast<int>( bond_timeout_.count( ) ) );
reset( true ); // hard reset to transition all still active down
// if a server crashed, it won't get cleared due to failed transition, clear manually
bond_map_.clear( );
// Initialize the bond respawn timer to check if server comes back online
// after a failure, within a maximum timeout period.
if ( attempt_respawn_reconnection_ ) {
bond_respawn_timer_ = this->create_wall_timer(
1s,
std::bind( &LifecycleManager::checkBondRespawnConnection, this ),
callback_group_ );
}
return;
}
}
}
void
422 LifecycleManager::checkBondRespawnConnection( )
{
// First attempt in respawn, start maximum duration to respawn
if ( bond_respawn_start_time_.nanoseconds( ) == 0 ) {
bond_respawn_start_time_ = now( );
}
// Note: system_active_ is inverted since this should be in a failure
// condition. If another outside user actives the system again, this should not process.
if ( system_active_ || !rclcpp::ok( ) || node_names_.empty( ) ) {
bond_respawn_start_time_ = rclcpp::Time( 0 );
bond_respawn_timer_.reset( );
return;
}
// Check number of live connections after a bond failure
int live_servers = 0;
const int max_live_servers = node_names_.size( );
for ( auto & node_name : node_names_ ) {
if ( !rclcpp::ok( ) ) {
return;
}
try {
node_map_[node_name]->get_state( ); // Only won't throw if the server exists
live_servers++;
} catch ( ... ) {
break;
}
}
// If all are alive, kill timer and retransition system to active
// Else, check if maximum timeout has occurred
if ( live_servers == max_live_servers ) {
message( "Successfully re-established connections from server respawns, starting back up." );
bond_respawn_start_time_ = rclcpp::Time( 0 );
bond_respawn_timer_.reset( );
startup( );
} else if ( now( ) - bond_respawn_start_time_ >= bond_respawn_max_duration_ ) {
message( "Failed to re-establish connection from a server crash after maximum timeout." );
bond_respawn_start_time_ = rclcpp::Time( 0 );
bond_respawn_timer_.reset( );
}
}
#define ANSI_COLOR_RESET "\x1b[0m"
#define ANSI_COLOR_BLUE "\x1b[34m"
void
471 LifecycleManager::message( const std::string & msg )
{
RCLCPP_INFO( get_logger( ), ANSI_COLOR_BLUE "\33[1m%s\33[0m" ANSI_COLOR_RESET, msg.c_str( ) );
}
} // namespace nav2_lifecycle_manager
#include "rclcpp_components/register_node_macro.hpp"
479 RCLCPP_COMPONENTS_REGISTER_NODE( nav2_lifecycle_manager::LifecycleManager )
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "nav2_lifecycle_manager/lifecycle_manager_client.hpp"
#include <cmath>
#include <memory>
#include <string>
#include <utility>
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "nav2_util/geometry_utils.hpp"
namespace nav2_lifecycle_manager
{
using nav2_util::geometry_utils::orientationAroundZAxis;
29 LifecycleManagerClient::LifecycleManagerClient(
30 const std::string & name,
31 std::shared_ptr<rclcpp::Node> parent_node )
{
manage_service_name_ = name + std::string( "/manage_nodes" );
active_service_name_ = name + std::string( "/is_active" );
// Use parent node for service call and logging
node_ = parent_node;
// Create the service clients
manager_client_ = std::make_shared<nav2_util::ServiceClient<ManageLifecycleNodes>>(
manage_service_name_, node_ );
is_active_client_ = std::make_shared<nav2_util::ServiceClient<std_srvs::srv::Trigger>>(
active_service_name_, node_ );
}
bool
47 LifecycleManagerClient::startup( const std::chrono::nanoseconds timeout )
{
return callService( ManageLifecycleNodes::Request::STARTUP, timeout );
}
bool
53 LifecycleManagerClient::shutdown( const std::chrono::nanoseconds timeout )
{
return callService( ManageLifecycleNodes::Request::SHUTDOWN, timeout );
}
bool
59 LifecycleManagerClient::pause( const std::chrono::nanoseconds timeout )
{
return callService( ManageLifecycleNodes::Request::PAUSE, timeout );
}
bool
65 LifecycleManagerClient::resume( const std::chrono::nanoseconds timeout )
{
return callService( ManageLifecycleNodes::Request::RESUME, timeout );
}
bool
71 LifecycleManagerClient::reset( const std::chrono::nanoseconds timeout )
{
return callService( ManageLifecycleNodes::Request::RESET, timeout );
}
SystemStatus
77 LifecycleManagerClient::is_active( const std::chrono::nanoseconds timeout )
{
auto request = std::make_shared<std_srvs::srv::Trigger::Request>( );
auto response = std::make_shared<std_srvs::srv::Trigger::Response>( );
RCLCPP_DEBUG(
node_->get_logger( ), "Waiting for the %s service...",
active_service_name_.c_str( ) );
if ( !is_active_client_->wait_for_service( std::chrono::seconds( 1 ) ) ) {
return SystemStatus::TIMEOUT;
}
RCLCPP_DEBUG(
node_->get_logger( ), "Sending %s request",
active_service_name_.c_str( ) );
try {
response = is_active_client_->invoke( request, timeout );
} catch ( std::runtime_error & ) {
return SystemStatus::TIMEOUT;
}
if ( response->success ) {
return SystemStatus::ACTIVE;
} else {
return SystemStatus::INACTIVE;
}
}
bool
108 LifecycleManagerClient::callService( uint8_t command, const std::chrono::nanoseconds timeout )
{
auto request = std::make_shared<ManageLifecycleNodes::Request>( );
request->command = command;
RCLCPP_DEBUG(
node_->get_logger( ), "Waiting for the %s service...",
manage_service_name_.c_str( ) );
while ( !manager_client_->wait_for_service( timeout ) ) {
if ( !rclcpp::ok( ) ) {
RCLCPP_ERROR( node_->get_logger( ), "Client interrupted while waiting for service to appear" );
return false;
}
RCLCPP_DEBUG( node_->get_logger( ), "Waiting for service to appear..." );
}
RCLCPP_DEBUG(
node_->get_logger( ), "Sending %s request",
manage_service_name_.c_str( ) );
try {
auto future_result = manager_client_->invoke( request, timeout );
return future_result->success;
} catch ( std::runtime_error & ) {
return false;
}
}
} // namespace nav2_lifecycle_manager
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include "nav2_lifecycle_manager/lifecycle_manager.hpp"
#include "rclcpp/rclcpp.hpp"
20 int main( int argc, char ** argv )
{
rclcpp::init( argc, argv );
auto node = std::make_shared<nav2_lifecycle_manager::LifecycleManager>( );
rclcpp::spin( node );
rclcpp::shutdown( );
return 0;
}
// Copyright ( c ) 2020 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <chrono>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/node_thread.hpp"
#include "nav2_lifecycle_manager/lifecycle_manager.hpp"
#include "nav2_lifecycle_manager/lifecycle_manager_client.hpp"
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
// minimal lifecycle node implementing bond as in rest of navigation servers
28 class TestLifecycleNode : public nav2_util::LifecycleNode
{
public:
31 TestLifecycleNode( bool bond, std::string name )
: nav2_util::LifecycleNode( name )
{
state = "";
enable_bond = bond;
}
CallbackReturn on_configure( const rclcpp_lifecycle::State & /*state*/ ) override
{
RCLCPP_INFO( get_logger( ), "Lifecycle Test node is Configured!" );
state = "configured";
return CallbackReturn::SUCCESS;
}
45 CallbackReturn on_activate( const rclcpp_lifecycle::State & /*state*/ ) override
{
RCLCPP_INFO( get_logger( ), "Lifecycle Test node is Activated!" );
state = "activated";
if ( enable_bond ) {
createBond( );
}
return CallbackReturn::SUCCESS;
}
55 CallbackReturn on_deactivate( const rclcpp_lifecycle::State & /*state*/ ) override
{
RCLCPP_INFO( get_logger( ), "Lifecycle Test node is Deactivated!" );
state = "deactivated";
if ( enable_bond ) {
destroyBond( );
}
return CallbackReturn::SUCCESS;
}
65 CallbackReturn on_cleanup( const rclcpp_lifecycle::State & /*state*/ ) override
{
RCLCPP_INFO( get_logger( ), "Lifecycle Test node is Cleanup!" );
state = "cleaned up";
return CallbackReturn::SUCCESS;
}
72 CallbackReturn on_shutdown( const rclcpp_lifecycle::State & /*state*/ ) override
{
RCLCPP_INFO( get_logger( ), "Lifecycle Test node is Shutdown!" );
state = "shut down";
return CallbackReturn::SUCCESS;
}
79 CallbackReturn on_error( const rclcpp_lifecycle::State & /*state*/ ) override
{
RCLCPP_INFO( get_logger( ), "Lifecycle Test node is encountered an error!" );
state = "errored";
return CallbackReturn::SUCCESS;
}
86 bool bondAllocated( )
{
return bond_ ? true : false;
}
91 void breakBond( )
{
bond_->breakBond( );
}
96 std::string getState( )
{
return state;
}
101 bool isBondEnabled( )
{
return enable_bond;
}
106 bool isBondConnected( )
{
return bondAllocated( ) ? !bond_->isBroken( ) : false;
}
std::string state;
bool enable_bond;
};
class TestFixture
{
public:
TestFixture( bool bond, std::string node_name )
{
lf_node_ = std::make_shared<TestLifecycleNode>( bond, node_name );
lf_thread_ = std::make_unique<nav2_util::NodeThread>( lf_node_->get_node_base_interface( ) );
}
std::shared_ptr<TestLifecycleNode> lf_node_;
std::unique_ptr<nav2_util::NodeThread> lf_thread_;
};
TEST( LifecycleBondTest, POSITIVE )
{
// let the lifecycle server come up
rclcpp::Rate( 1 ).sleep( );
auto node = std::make_shared<rclcpp::Node>( "lifecycle_manager_test_service_client" );
nav2_lifecycle_manager::LifecycleManagerClient client( "lifecycle_manager_test", node );
// create node, should be up now
auto fixture = TestFixture( true, "bond_tester" );
auto bond_tester = fixture.lf_node_;
EXPECT_TRUE( client.startup( ) );
// check if bond is connected after being activated
rclcpp::Rate( 5 ).sleep( );
EXPECT_TRUE( bond_tester->isBondConnected( ) );
EXPECT_EQ( bond_tester->getState( ), "activated" );
bond_tester->breakBond( );
// bond should be disconnected now and lifecycle manager should know and react to reset
rclcpp::Rate( 5 ).sleep( );
EXPECT_EQ(
nav2_lifecycle_manager::SystemStatus::INACTIVE,
client.is_active( std::chrono::nanoseconds( 1000000000 ) ) );
EXPECT_FALSE( bond_tester->isBondConnected( ) );
EXPECT_EQ( bond_tester->getState( ), "cleaned up" );
// check that bringing up again is OK
EXPECT_TRUE( client.startup( ) );
EXPECT_EQ( bond_tester->getState( ), "activated" );
EXPECT_TRUE( bond_tester->isBondConnected( ) );
EXPECT_EQ(
nav2_lifecycle_manager::SystemStatus::ACTIVE,
client.is_active( std::chrono::nanoseconds( 1000000000 ) ) );
// clean state for next test.
EXPECT_TRUE( client.reset( ) );
EXPECT_FALSE( bond_tester->isBondConnected( ) );
EXPECT_EQ( bond_tester->getState( ), "cleaned up" );
}
TEST( LifecycleBondTest, NEGATIVE )
{
auto node = std::make_shared<rclcpp::Node>( "lifecycle_manager_test_service_client" );
nav2_lifecycle_manager::LifecycleManagerClient client( "lifecycle_manager_test", node );
// create node, now without bond setup to connect to. Should fail because no bond
auto fixture = TestFixture( false, "bond_tester" );
auto bond_tester = fixture.lf_node_;
EXPECT_FALSE( client.startup( ) );
EXPECT_FALSE( bond_tester->isBondEnabled( ) );
EXPECT_EQ(
nav2_lifecycle_manager::SystemStatus::INACTIVE,
client.is_active( std::chrono::nanoseconds( 1000000000 ) ) );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
// Copyright ( c ) 2020 Shivang Patel
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "nav2_util/node_thread.hpp"
#include "nav2_lifecycle_manager/lifecycle_manager_client.hpp"
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
25 class LifecycleNodeTest : public rclcpp_lifecycle::LifecycleNode
{
public:
28 LifecycleNodeTest( )
: rclcpp_lifecycle::LifecycleNode( "lifecycle_node_test" ) {}
CallbackReturn on_configure( const rclcpp_lifecycle::State & /*state*/ ) override
{
RCLCPP_INFO( get_logger( ), "Lifecycle Test node is Configured!" );
return CallbackReturn::SUCCESS;
}
37 CallbackReturn on_activate( const rclcpp_lifecycle::State & /*state*/ ) override
{
RCLCPP_INFO( get_logger( ), "Lifecycle Test node is Activated!" );
return CallbackReturn::SUCCESS;
}
43 CallbackReturn on_deactivate( const rclcpp_lifecycle::State & /*state*/ ) override
{
RCLCPP_INFO( get_logger( ), "Lifecycle Test node is Deactivated!" );
return CallbackReturn::SUCCESS;
}
49 CallbackReturn on_cleanup( const rclcpp_lifecycle::State & /*state*/ ) override
{
RCLCPP_INFO( get_logger( ), "Lifecycle Test node is Cleanup!" );
return CallbackReturn::SUCCESS;
}
55 CallbackReturn on_shutdown( const rclcpp_lifecycle::State & /*state*/ ) override
{
RCLCPP_INFO( get_logger( ), "Lifecycle Test node is Shutdown!" );
return CallbackReturn::SUCCESS;
}
61 CallbackReturn on_error( const rclcpp_lifecycle::State & /*state*/ ) override
{
RCLCPP_INFO( get_logger( ), "Lifecycle Test node is encountered an error!" );
return CallbackReturn::SUCCESS;
}
};
class LifecycleClientTestFixture
{
public:
LifecycleClientTestFixture( )
{
lf_node_ = std::make_shared<LifecycleNodeTest>( );
lf_thread_ = std::make_unique<nav2_util::NodeThread>( lf_node_->get_node_base_interface( ) );
}
private:
std::shared_ptr<LifecycleNodeTest> lf_node_;
std::unique_ptr<nav2_util::NodeThread> lf_thread_;
};
TEST( LifecycleClientTest, BasicTest )
{
LifecycleClientTestFixture fix;
auto node = std::make_shared<rclcpp::Node>( "lifecycle_manager_test_service_client" );
nav2_lifecycle_manager::LifecycleManagerClient client( "lifecycle_manager_test", node );
EXPECT_EQ(
nav2_lifecycle_manager::SystemStatus::TIMEOUT,
client.is_active( std::chrono::nanoseconds( 1000 ) ) );
EXPECT_TRUE( client.startup( ) );
EXPECT_EQ(
nav2_lifecycle_manager::SystemStatus::ACTIVE,
client.is_active( std::chrono::nanoseconds( 1000000000 ) ) );
EXPECT_EQ(
nav2_lifecycle_manager::SystemStatus::ACTIVE,
client.is_active( ) );
EXPECT_TRUE( client.pause( ) );
EXPECT_EQ(
nav2_lifecycle_manager::SystemStatus::INACTIVE,
client.is_active( std::chrono::nanoseconds( 1000000000 ) ) );
EXPECT_TRUE( client.resume( ) );
EXPECT_TRUE( client.reset( ) );
EXPECT_TRUE( client.shutdown( ) );
}
TEST( LifecycleClientTest, WithoutFixture )
{
auto node = std::make_shared<rclcpp::Node>( "lifecycle_manager_test_service_client" );
nav2_lifecycle_manager::LifecycleManagerClient client( "lifecycle_manager_test", node );
EXPECT_EQ(
nav2_lifecycle_manager::SystemStatus::TIMEOUT,
client.is_active( std::chrono::nanoseconds( 1000 ) ) );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2020 Samsung Research Russia
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_MAP_SERVER__COSTMAP_FILTER_INFO_SERVER_HPP_
#define NAV2_MAP_SERVER__COSTMAP_FILTER_INFO_SERVER_HPP_
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_msgs/msg/costmap_filter_info.hpp"
namespace nav2_map_server
{
27 class CostmapFilterInfoServer : public nav2_util::LifecycleNode
{
public:
/**
* @brief Constructor for the nav2_map_server::CostmapFilterInfoServer
*/
33 CostmapFilterInfoServer( );
/**
* @brief Destructor for the nav2_map_server::CostmapFilterInfoServer
*/
37 ~CostmapFilterInfoServer( );
protected:
/**
* @brief Creates CostmapFilterInfo publisher and forms published message from ROS parameters
* @param state Lifecycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_configure( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Publishes a CostmapFilterInfo message
* @param state Lifecycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_activate( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Deactivates publisher
* @param state Lifecycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Resets publisher
* @param state Lifecycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Called when in Shutdown state
* @param state Lifecycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_shutdown( const rclcpp_lifecycle::State & state ) override;
private:
rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::CostmapFilterInfo>::SharedPtr publisher_;
std::unique_ptr<nav2_msgs::msg::CostmapFilterInfo> msg_;
}; // CostmapFilterInfoServer
} // namespace nav2_map_server
#endif // NAV2_MAP_SERVER__COSTMAP_FILTER_INFO_SERVER_HPP_
1 // Copyright ( c ) 2019 Rover Robotics
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_MAP_SERVER__MAP_MODE_HPP_
#define NAV2_MAP_SERVER__MAP_MODE_HPP_
#include <string>
#include <vector>
namespace nav2_map_server
{
/**
* @enum nav2_map_server::MapMode
* @brief Describes the relation between image pixel values and map occupancy
* status ( 0-100; -1 ). Lightness refers to the mean of a given pixel's RGB
* channels on a scale from 0 to 255.
*/
28 enum class MapMode
{
/**
* Together with associated threshold values ( occupied and free ):
* lightness >= occupied threshold - Occupied ( 100 )
* ... ( anything in between ) - Unknown ( -1 )
* lightness <= free threshold - Free ( 0 )
*/
Trinary,
/**
* Together with associated threshold values ( occupied and free ):
* alpha < 1.0 - Unknown ( -1 )
* lightness >= occ_th - Occupied ( 100 )
* ... ( linearly interpolate to )
* lightness <= free_th - Free ( 0 )
*/
Scale,
/**
* Lightness = 0 - Free ( 0 )
* ... ( linearly interpolate to )
* Lightness = 100 - Occupied ( 100 )
* Lightness >= 101 - Unknown
*/
Raw,
};
/**
* @brief Convert a MapMode enum to the name of the map mode
* @param map_mode Mode for the map
* @return String identifier of the given map mode
* @throw std::invalid_argument if the given value is not a defined map mode
*/
60 const char * map_mode_to_string( MapMode map_mode );
/**
* @brief Convert the name of a map mode to a MapMode enum
* @param map_mode_name Name of the map mode
* @throw std::invalid_argument if the name does not name a map mode
* @return map mode corresponding to the string
*/
68 MapMode map_mode_from_string( std::string map_mode_name );
} // namespace nav2_map_server
#endif // NAV2_MAP_SERVER__MAP_MODE_HPP_
// Copyright ( c ) 2020 Samsung Research Russia
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_MAP_SERVER__MAP_SAVER_HPP_
#define NAV2_MAP_SERVER__MAP_SAVER_HPP_
#include <string>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_msgs/srv/save_map.hpp"
#include "map_io.hpp"
namespace nav2_map_server
{
/**
* @class nav2_map_server::MapSaver
* @brief A class that provides map saving methods and services
*/
35 class MapSaver : public nav2_util::LifecycleNode
{
public:
/**
* @brief Constructor for the nav2_map_server::MapSaver
* @param options Additional options to control creation of the node.
*/
42 explicit MapSaver( const rclcpp::NodeOptions & options = rclcpp::NodeOptions( ) );
/**
* @brief Destructor for the nav2_map_server::MapServer
*/
47 ~MapSaver( );
/**
* @brief Read a message from incoming map topic and save map to a file
* @param map_topic Incoming map topic name
* @param save_parameters Map saving parameters.
* @return true of false
*/
55 bool saveMapTopicToFile(
56 const std::string & map_topic,
57 const SaveParameters & save_parameters );
/**
* @brief Sets up map saving service
* @param state Lifecycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_configure( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Called when node switched to active state
* @param state Lifecycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_activate( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Called when node switched to inactive state
* @param state Lifecycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Called when it is required node clean-up
* @param state Lifecycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Called when in Shutdown state
* @param state Lifecycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_shutdown( const rclcpp_lifecycle::State & state ) override;
protected:
/**
* @brief Map saving service callback
* @param request_header Service request header
* @param request Service request
* @param response Service response
*/
void saveMapCallback(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<nav2_msgs::srv::SaveMap::Request> request,
std::shared_ptr<nav2_msgs::srv::SaveMap::Response> response );
// The timeout for saving the map in service
std::shared_ptr<rclcpp::Duration> save_map_timeout_;
// Default values for map thresholds
double free_thresh_default_;
double occupied_thresh_default_;
// param for handling QoS configuration
bool map_subscribe_transient_local_;
// The name of the service for saving a map from topic
const std::string save_map_service_name_{"save_map"};
// A service to save the map to a file at run time ( SaveMap )
rclcpp::Service<nav2_msgs::srv::SaveMap>::SharedPtr save_map_service_;
};
} // namespace nav2_map_server
#endif // NAV2_MAP_SERVER__MAP_SAVER_HPP_
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_MAP_SERVER__MAP_SERVER_HPP_
#define NAV2_MAP_SERVER__MAP_SERVER_HPP_
#include <string>
#include <memory>
#include <functional>
#include "rclcpp/rclcpp.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "nav_msgs/srv/get_map.hpp"
#include "nav2_msgs/srv/load_map.hpp"
namespace nav2_map_server
{
/**
* @class nav2_map_server::MapServer
* @brief Parses the map yaml file and creates a service and a publisher that
* provides occupancy grid
*/
36 class MapServer : public nav2_util::LifecycleNode
{
public:
/**
* @brief A constructor for nav2_map_server::MapServer
* @param options Additional options to control creation of the node.
*/
43 explicit MapServer( const rclcpp::NodeOptions & options = rclcpp::NodeOptions( ) );
/**
* @brief A Destructor for nav2_map_server::MapServer
*/
48 ~MapServer( );
protected:
/**
* @brief Sets up required params and services. Loads map and its parameters from the file
* @param state Lifecycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_configure( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Start publishing the map using the latched topic
* @param state Lifecycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_activate( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Stops publishing the latched topic
* @param state Lifecycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Resets the member variables
* @param state Lifecycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Called when in Shutdown state
* @param state Lifecycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_shutdown( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Load the map YAML, image from map file name and
* generate output response containing an OccupancyGrid.
* Update msg_ class variable.
* @param yaml_file name of input YAML file
* @param response Output response with loaded OccupancyGrid map
* @return true or false
*/
bool loadMapResponseFromYaml(
const std::string & yaml_file,
std::shared_ptr<nav2_msgs::srv::LoadMap::Response> response );
/**
* @brief Method correcting msg_ header when it belongs to instantiated object
*/
void updateMsgHeader( );
/**
* @brief Map getting service callback
* @param request_header Service request header
* @param request Service request
* @param response Service response
*/
void getMapCallback(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<nav_msgs::srv::GetMap::Request> request,
std::shared_ptr<nav_msgs::srv::GetMap::Response> response );
/**
* @brief Map loading service callback
* @param request_header Service request header
* @param request Service request
* @param response Service response
*/
void loadMapCallback(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<nav2_msgs::srv::LoadMap::Request> request,
std::shared_ptr<nav2_msgs::srv::LoadMap::Response> response );
// The name of the service for getting a map
const std::string service_name_{"map"};
// The name of the service for loading a map
const std::string load_map_service_name_{"load_map"};
// A service to provide the occupancy grid ( GetMap ) and the message to return
rclcpp::Service<nav_msgs::srv::GetMap>::SharedPtr occ_service_;
// A service to load the occupancy grid from file at run time ( LoadMap )
rclcpp::Service<nav2_msgs::srv::LoadMap>::SharedPtr load_map_service_;
// A topic on which the occupancy grid will be published
rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>::SharedPtr occ_pub_;
// The frame ID used in the returned OccupancyGrid message
std::string frame_id_;
// The message to publish on the occupancy grid topic
nav_msgs::msg::OccupancyGrid msg_;
// true if msg_ was initialized
bool map_available_;
};
} // namespace nav2_map_server
#endif // NAV2_MAP_SERVER__MAP_SERVER_HPP_
1 // Copyright ( c ) 2020 Samsung Research Russia
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// TODO( AlexeyMerzlyakov ): This dummy info publisher should be removed
// after Semantic Map Server having the same functionality will be developed.
#include "nav2_map_server/costmap_filter_info_server.hpp"
#include <string>
#include <memory>
#include <utility>
namespace nav2_map_server
{
27 CostmapFilterInfoServer::CostmapFilterInfoServer( )
: nav2_util::LifecycleNode( "costmap_filter_info_server" )
{
declare_parameter( "filter_info_topic", "costmap_filter_info" );
declare_parameter( "type", 0 );
declare_parameter( "mask_topic", "filter_mask" );
declare_parameter( "base", 0.0 );
declare_parameter( "multiplier", 1.0 );
}
37 CostmapFilterInfoServer::~CostmapFilterInfoServer( )
{
}
nav2_util::CallbackReturn
42 CostmapFilterInfoServer::on_configure( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Configuring" );
std::string filter_info_topic = get_parameter( "filter_info_topic" ).as_string( );
publisher_ = this->create_publisher<nav2_msgs::msg::CostmapFilterInfo>(
filter_info_topic, rclcpp::QoS( rclcpp::KeepLast( 1 ) ).transient_local( ).reliable( ) );
msg_ = std::make_unique<nav2_msgs::msg::CostmapFilterInfo>( );
msg_->header.frame_id = "";
msg_->header.stamp = now( );
msg_->type = get_parameter( "type" ).as_int( );
msg_->filter_mask_topic = get_parameter( "mask_topic" ).as_string( );
msg_->base = static_cast<float>( get_parameter( "base" ).as_double( ) );
msg_->multiplier = static_cast<float>( get_parameter( "multiplier" ).as_double( ) );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
63 CostmapFilterInfoServer::on_activate( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Activating" );
publisher_->on_activate( );
publisher_->publish( std::move( msg_ ) );
// create bond connection
createBond( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
77 CostmapFilterInfoServer::on_deactivate( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Deactivating" );
publisher_->on_deactivate( );
// destroy bond connection
destroyBond( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
90 CostmapFilterInfoServer::on_cleanup( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Cleaning up" );
publisher_.reset( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
100 CostmapFilterInfoServer::on_shutdown( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Shutting down" );
return nav2_util::CallbackReturn::SUCCESS;
}
} // namespace nav2_map_server
1 // Copyright ( c ) 2020 Samsung Research Russia
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include "nav2_map_server/costmap_filter_info_server.hpp"
#include "rclcpp/rclcpp.hpp"
20 int main( int argc, char * argv[] )
{
auto logger = rclcpp::get_logger( "costmap_filter_info_server" );
RCLCPP_INFO( logger, "This is costmap filter info publisher" );
rclcpp::init( argc, argv );
auto node = std::make_shared<nav2_map_server::CostmapFilterInfoServer>( );
rclcpp::spin( node->get_node_base_interface( ) );
rclcpp::shutdown( );
return 0;
}
1 /* Copyright 2019 Rover Robotics
* Copyright 2010 Brian Gerkey
* Copyright ( c ) 2008, Willow Garage, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the <ORGANIZATION> nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES ( INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE )
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "nav2_map_server/map_io.hpp"
#ifndef _WIN32
#include <libgen.h>
#endif
#include <iostream>
#include <string>
#include <vector>
#include <fstream>
#include <stdexcept>
#include "Magick++.h"
#include "nav2_util/geometry_utils.hpp"
#include "yaml-cpp/yaml.h"
#include "tf2/LinearMath/Matrix3x3.h"
#include "tf2/LinearMath/Quaternion.h"
#include "nav2_util/occ_grid_values.hpp"
#ifdef _WIN32
// https://github.com/rtv/Stage/blob/master/replace/dirname.c
static
54 char * dirname( char * path )
{
static const char dot[] = ".";
char * last_slash;
if ( path == NULL ) {
return path;
}
/* Replace all "\" with "/" */
char * c = path;
while ( *c != '\0' ) {
if ( *c == '\\' ) {*c = '/';}
++c;
}
/* Find last '/'. */
last_slash = path != NULL ? strrchr( path, '/' ) : NULL;
if ( last_slash != NULL && last_slash == path ) {
/* The last slash is the first character in the string. We have to
return "/". */
++last_slash;
} else if ( last_slash != NULL && last_slash[1] == '\0' ) {
/* The '/' is the last character, we have to look further. */
last_slash = reinterpret_cast<char *>( memchr( path, last_slash - path, '/' ) );
}
if ( last_slash != NULL ) {
/* Terminate the path. */
last_slash[0] = '\0';
} else {
/* This assignment is ill-designed but the XPG specs require to
return a string containing "." in any case no directory part is
found and so a static and constant string is required. */
path = reinterpret_cast<char *>( dot );
}
return path;
}
#endif
namespace nav2_map_server
{
using nav2_util::geometry_utils::orientationAroundZAxis;
// === Map input part ===
/// Get the given subnode value.
/// The only reason this function exists is to wrap the exceptions in slightly nicer error messages,
/// including the name of the failed key
/// @throw YAML::Exception
template<typename T>
107 T yaml_get_value( const YAML::Node & node, const std::string & key )
{
try {
return node[key].as<T>( );
} catch ( YAML::Exception & e ) {
std::stringstream ss;
ss << "Failed to parse YAML tag '" << key << "' for reason: " << e.msg;
throw YAML::Exception( e.mark, ss.str( ) );
}
}
118 LoadParameters loadMapYaml( const std::string & yaml_filename )
{
YAML::Node doc = YAML::LoadFile( yaml_filename );
LoadParameters load_parameters;
auto image_file_name = yaml_get_value<std::string>( doc, "image" );
if ( image_file_name.empty( ) ) {
throw YAML::Exception( doc["image"].Mark( ), "The image tag was empty." );
}
if ( image_file_name[0] != '/' ) {
// dirname takes a mutable char *, so we copy into a vector
std::vector<char> fname_copy( yaml_filename.begin( ), yaml_filename.end( ) );
fname_copy.push_back( '\0' );
image_file_name = std::string( dirname( fname_copy.data( ) ) ) + '/' + image_file_name;
}
load_parameters.image_file_name = image_file_name;
load_parameters.resolution = yaml_get_value<double>( doc, "resolution" );
load_parameters.origin = yaml_get_value<std::vector<double>>( doc, "origin" );
if ( load_parameters.origin.size( ) != 3 ) {
throw YAML::Exception(
doc["origin"].Mark( ), "value of the 'origin' tag should have 3 elements, not " +
std::to_string( load_parameters.origin.size( ) ) );
}
load_parameters.free_thresh = yaml_get_value<double>( doc, "free_thresh" );
load_parameters.occupied_thresh = yaml_get_value<double>( doc, "occupied_thresh" );
auto map_mode_node = doc["mode"];
if ( !map_mode_node.IsDefined( ) ) {
load_parameters.mode = MapMode::Trinary;
} else {
load_parameters.mode = map_mode_from_string( map_mode_node.as<std::string>( ) );
}
try {
load_parameters.negate = yaml_get_value<int>( doc, "negate" );
} catch ( YAML::Exception & ) {
load_parameters.negate = yaml_get_value<bool>( doc, "negate" );
}
std::cout << "[DEBUG] [map_io]: resolution: " << load_parameters.resolution << std::endl;
std::cout << "[DEBUG] [map_io]: origin[0]: " << load_parameters.origin[0] << std::endl;
std::cout << "[DEBUG] [map_io]: origin[1]: " << load_parameters.origin[1] << std::endl;
std::cout << "[DEBUG] [map_io]: origin[2]: " << load_parameters.origin[2] << std::endl;
std::cout << "[DEBUG] [map_io]: free_thresh: " << load_parameters.free_thresh << std::endl;
std::cout << "[DEBUG] [map_io]: occupied_thresh: " << load_parameters.occupied_thresh <<
std::endl;
std::cout << "[DEBUG] [map_io]: mode: " << map_mode_to_string( load_parameters.mode ) << std::endl;
std::cout << "[DEBUG] [map_io]: negate: " << load_parameters.negate << std::endl; //NOLINT
return load_parameters;
}
172 void loadMapFromFile(
const LoadParameters & load_parameters,
nav_msgs::msg::OccupancyGrid & map )
{
Magick::InitializeMagick( nullptr );
nav_msgs::msg::OccupancyGrid msg;
std::cout << "[INFO] [map_io]: Loading image_file: " <<
load_parameters.image_file_name << std::endl;
Magick::Image img( load_parameters.image_file_name );
// Copy the image data into the map structure
msg.info.width = img.size( ).width( );
msg.info.height = img.size( ).height( );
msg.info.resolution = load_parameters.resolution;
msg.info.origin.position.x = load_parameters.origin[0];
msg.info.origin.position.y = load_parameters.origin[1];
msg.info.origin.position.z = 0.0;
msg.info.origin.orientation = orientationAroundZAxis( load_parameters.origin[2] );
// Allocate space to hold the data
msg.data.resize( msg.info.width * msg.info.height );
// Copy pixel data into the map structure
for ( size_t y = 0; y < msg.info.height; y++ ) {
for ( size_t x = 0; x < msg.info.width; x++ ) {
auto pixel = img.pixelColor( x, y );
std::vector<Magick::Quantum> channels = {pixel.redQuantum( ), pixel.greenQuantum( ),
pixel.blueQuantum( )};
if ( load_parameters.mode == MapMode::Trinary && img.matte( ) ) {
// To preserve existing behavior, average in alpha with color channels in Trinary mode.
// CAREFUL. alpha is inverted from what you might expect. High = transparent, low = opaque
channels.push_back( MaxRGB - pixel.alphaQuantum( ) );
}
double sum = 0;
for ( auto c : channels ) {
sum += c;
}
/// on a scale from 0.0 to 1.0 how bright is the pixel?
double shade = Magick::ColorGray::scaleQuantumToDouble( sum / channels.size( ) );
// If negate is true, we consider blacker pixels free, and whiter
// pixels occupied. Otherwise, it's vice versa.
/// on a scale from 0.0 to 1.0, how occupied is the map cell ( before thresholding )?
double occ = ( load_parameters.negate ? shade : 1.0 - shade );
int8_t map_cell;
switch ( load_parameters.mode ) {
case MapMode::Trinary:
if ( load_parameters.occupied_thresh < occ ) {
map_cell = nav2_util::OCC_GRID_OCCUPIED;
} else if ( occ < load_parameters.free_thresh ) {
map_cell = nav2_util::OCC_GRID_FREE;
} else {
map_cell = nav2_util::OCC_GRID_UNKNOWN;
}
break;
case MapMode::Scale:
if ( pixel.alphaQuantum( ) != OpaqueOpacity ) {
map_cell = nav2_util::OCC_GRID_UNKNOWN;
} else if ( load_parameters.occupied_thresh < occ ) {
map_cell = nav2_util::OCC_GRID_OCCUPIED;
} else if ( occ < load_parameters.free_thresh ) {
map_cell = nav2_util::OCC_GRID_FREE;
} else {
map_cell = std::rint(
( occ - load_parameters.free_thresh ) /
( load_parameters.occupied_thresh - load_parameters.free_thresh ) * 100.0 );
}
break;
case MapMode::Raw: {
double occ_percent = std::round( shade * 255 );
if ( nav2_util::OCC_GRID_FREE <= occ_percent &&
occ_percent <= nav2_util::OCC_GRID_OCCUPIED )
{
map_cell = static_cast<int8_t>( occ_percent );
} else {
map_cell = nav2_util::OCC_GRID_UNKNOWN;
}
break;
}
default:
throw std::runtime_error( "Invalid map mode" );
}
msg.data[msg.info.width * ( msg.info.height - y - 1 ) + x] = map_cell;
}
}
// Since loadMapFromFile( ) does not belong to any node, publishing in a system time.
rclcpp::Clock clock( RCL_SYSTEM_TIME );
msg.info.map_load_time = clock.now( );
msg.header.frame_id = "map";
msg.header.stamp = clock.now( );
std::cout <<
"[DEBUG] [map_io]: Read map " << load_parameters.image_file_name << ": " << msg.info.width <<
" X " << msg.info.height << " map @ " << msg.info.resolution << " m/cell" << std::endl;
map = msg;
}
275 LOAD_MAP_STATUS loadMapFromYaml(
const std::string & yaml_file,
nav_msgs::msg::OccupancyGrid & map )
{
if ( yaml_file.empty( ) ) {
std::cerr << "[ERROR] [map_io]: YAML file name is empty, can't load!" << std::endl;
return MAP_DOES_NOT_EXIST;
}
std::cout << "[INFO] [map_io]: Loading yaml file: " << yaml_file << std::endl;
LoadParameters load_parameters;
try {
load_parameters = loadMapYaml( yaml_file );
} catch ( YAML::Exception & e ) {
std::cerr <<
"[ERROR] [map_io]: Failed processing YAML file " << yaml_file << " at position ( " <<
e.mark.line << ":" << e.mark.column << " ) for reason: " << e.what( ) << std::endl;
return INVALID_MAP_METADATA;
} catch ( std::exception & e ) {
std::cerr <<
"[ERROR] [map_io]: Failed to parse map YAML loaded from file " << yaml_file <<
" for reason: " << e.what( ) << std::endl;
return INVALID_MAP_METADATA;
}
try {
loadMapFromFile( load_parameters, map );
} catch ( std::exception & e ) {
std::cerr <<
"[ERROR] [map_io]: Failed to load image file " << load_parameters.image_file_name <<
" for reason: " << e.what( ) << std::endl;
return INVALID_MAP_DATA;
}
return LOAD_MAP_SUCCESS;
}
// === Map output part ===
/**
* @brief Checks map saving parameters for consistency
* @param save_parameters Map saving parameters.
* NOTE: save_parameters could be updated during function execution.
* @throw std::exception in case of inconsistent parameters
*/
319 void checkSaveParameters( SaveParameters & save_parameters )
{
// Magick must me initialized before any activity with images
Magick::InitializeMagick( nullptr );
// Checking map file name
if ( save_parameters.map_file_name == "" ) {
rclcpp::Clock clock( RCL_SYSTEM_TIME );
save_parameters.map_file_name = "map_" +
std::to_string( static_cast<int>( clock.now( ).seconds( ) ) );
std::cout << "[WARN] [map_io]: Map file unspecified. Map will be saved to " <<
save_parameters.map_file_name << " file" << std::endl;
}
// Checking thresholds
if ( save_parameters.occupied_thresh == 0.0 ) {
save_parameters.occupied_thresh = 0.65;
std::cout << "[WARN] [map_io]: Occupied threshold unspecified. Setting it to default value: " <<
save_parameters.occupied_thresh << std::endl;
}
if ( save_parameters.free_thresh == 0.0 ) {
save_parameters.free_thresh = 0.25;
std::cout << "[WARN] [map_io]: Free threshold unspecified. Setting it to default value: " <<
save_parameters.free_thresh << std::endl;
}
if ( 1.0 < save_parameters.occupied_thresh ) {
std::cerr << "[ERROR] [map_io]: Threshold_occupied must be 1.0 or less" << std::endl;
throw std::runtime_error( "Incorrect thresholds" );
}
if ( save_parameters.free_thresh < 0.0 ) {
std::cerr << "[ERROR] [map_io]: Free threshold must be 0.0 or greater" << std::endl;
throw std::runtime_error( "Incorrect thresholds" );
}
if ( save_parameters.occupied_thresh <= save_parameters.free_thresh ) {
std::cerr << "[ERROR] [map_io]: Threshold_free must be smaller than threshold_occupied" <<
std::endl;
throw std::runtime_error( "Incorrect thresholds" );
}
// Checking image format
if ( save_parameters.image_format == "" ) {
save_parameters.image_format = save_parameters.mode == MapMode::Scale ? "png" : "pgm";
std::cout << "[WARN] [map_io]: Image format unspecified. Setting it to: " <<
save_parameters.image_format << std::endl;
}
std::transform(
save_parameters.image_format.begin( ),
save_parameters.image_format.end( ),
save_parameters.image_format.begin( ),
[]( unsigned char c ) {return std::tolower( c );} );
const std::vector<std::string> BLESSED_FORMATS{"bmp", "pgm", "png"};
if (
std::find( BLESSED_FORMATS.begin( ), BLESSED_FORMATS.end( ), save_parameters.image_format ) ==
BLESSED_FORMATS.end( ) )
{
std::stringstream ss;
bool first = true;
for ( auto & format_name : BLESSED_FORMATS ) {
if ( !first ) {
ss << ", ";
}
ss << "'" << format_name << "'";
first = false;
}
std::cout <<
"[WARN] [map_io]: Requested image format '" << save_parameters.image_format <<
"' is not one of the recommended formats: " << ss.str( ) << std::endl;
}
const std::string FALLBACK_FORMAT = "png";
try {
Magick::CoderInfo info( save_parameters.image_format );
if ( !info.isWritable( ) ) {
std::cout <<
"[WARN] [map_io]: Format '" << save_parameters.image_format <<
"' is not writable. Using '" << FALLBACK_FORMAT << "' instead" << std::endl;
save_parameters.image_format = FALLBACK_FORMAT;
}
} catch ( Magick::ErrorOption & e ) {
std::cout <<
"[WARN] [map_io]: Format '" << save_parameters.image_format << "' is not usable. Using '" <<
FALLBACK_FORMAT << "' instead:" << std::endl << e.what( ) << std::endl;
save_parameters.image_format = FALLBACK_FORMAT;
}
// Checking map mode
if (
save_parameters.mode == MapMode::Scale &&
( save_parameters.image_format == "pgm" ||
save_parameters.image_format == "jpg" ||
save_parameters.image_format == "jpeg" ) )
{
std::cout <<
"[WARN] [map_io]: Map mode 'scale' requires transparency, but format '" <<
save_parameters.image_format <<
"' does not support it. Consider switching image format to 'png'." << std::endl;
}
}
/**
* @brief Tries to write map data into a file
* @param map Occupancy grid data
* @param save_parameters Map saving parameters
* @throw std::expection in case of problem
*/
426 void tryWriteMapToFile(
const nav_msgs::msg::OccupancyGrid & map,
const SaveParameters & save_parameters )
{
std::cout <<
"[INFO] [map_io]: Received a " << map.info.width << " X " << map.info.height << " map @ " <<
map.info.resolution << " m/pix" << std::endl;
std::string mapdatafile = save_parameters.map_file_name + "." + save_parameters.image_format;
{
// should never see this color, so the initialization value is just for debugging
Magick::Image image( {map.info.width, map.info.height}, "red" );
// In scale mode, we need the alpha ( matte ) channel. Else, we don't.
// NOTE: GraphicsMagick seems to have trouble loading the alpha channel when saved with
// Magick::GreyscaleMatte, so we use TrueColorMatte instead.
image.type(
save_parameters.mode == MapMode::Scale ?
Magick::TrueColorMatteType : Magick::GrayscaleType );
// Since we only need to support 100 different pixel levels, 8 bits is fine
image.depth( 8 );
int free_thresh_int = std::rint( save_parameters.free_thresh * 100.0 );
int occupied_thresh_int = std::rint( save_parameters.occupied_thresh * 100.0 );
for ( size_t y = 0; y < map.info.height; y++ ) {
for ( size_t x = 0; x < map.info.width; x++ ) {
int8_t map_cell = map.data[map.info.width * ( map.info.height - y - 1 ) + x];
Magick::Color pixel;
switch ( save_parameters.mode ) {
case MapMode::Trinary:
if ( map_cell < 0 || 100 < map_cell ) {
pixel = Magick::ColorGray( 205 / 255.0 );
} else if ( map_cell <= free_thresh_int ) {
pixel = Magick::ColorGray( 254 / 255.0 );
} else if ( occupied_thresh_int <= map_cell ) {
pixel = Magick::ColorGray( 0 / 255.0 );
} else {
pixel = Magick::ColorGray( 205 / 255.0 );
}
break;
case MapMode::Scale:
if ( map_cell < 0 || 100 < map_cell ) {
pixel = Magick::ColorGray{0.5};
pixel.alphaQuantum( TransparentOpacity );
} else {
pixel = Magick::ColorGray{( 100.0 - map_cell ) / 100.0};
}
break;
case MapMode::Raw:
Magick::Quantum q;
if ( map_cell < 0 || 100 < map_cell ) {
q = MaxRGB;
} else {
q = map_cell / 255.0 * MaxRGB;
}
pixel = Magick::Color( q, q, q );
break;
default:
std::cerr << "[ERROR] [map_io]: Map mode should be Trinary, Scale or Raw" << std::endl;
throw std::runtime_error( "Invalid map mode" );
}
image.pixelColor( x, y, pixel );
}
}
std::cout << "[INFO] [map_io]: Writing map occupancy data to " << mapdatafile << std::endl;
image.write( mapdatafile );
}
std::string mapmetadatafile = save_parameters.map_file_name + ".yaml";
{
std::ofstream yaml( mapmetadatafile );
geometry_msgs::msg::Quaternion orientation = map.info.origin.orientation;
tf2::Matrix3x3 mat( tf2::Quaternion( orientation.x, orientation.y, orientation.z, orientation.w ) );
double yaw, pitch, roll;
mat.getEulerYPR( yaw, pitch, roll );
YAML::Emitter e;
e << YAML::Precision( 3 );
e << YAML::BeginMap;
e << YAML::Key << "image" << YAML::Value << mapdatafile;
e << YAML::Key << "mode" << YAML::Value << map_mode_to_string( save_parameters.mode );
e << YAML::Key << "resolution" << YAML::Value << map.info.resolution;
e << YAML::Key << "origin" << YAML::Flow << YAML::BeginSeq << map.info.origin.position.x <<
map.info.origin.position.y << yaw << YAML::EndSeq;
e << YAML::Key << "negate" << YAML::Value << 0;
e << YAML::Key << "occupied_thresh" << YAML::Value << save_parameters.occupied_thresh;
e << YAML::Key << "free_thresh" << YAML::Value << save_parameters.free_thresh;
if ( !e.good( ) ) {
std::cout <<
"[WARN] [map_io]: YAML writer failed with an error " << e.GetLastError( ) <<
". The map metadata may be invalid." << std::endl;
}
std::cout << "[INFO] [map_io]: Writing map metadata to " << mapmetadatafile << std::endl;
std::ofstream( mapmetadatafile ) << e.c_str( );
}
std::cout << "[INFO] [map_io]: Map saved" << std::endl;
}
532 bool saveMapToFile(
const nav_msgs::msg::OccupancyGrid & map,
const SaveParameters & save_parameters )
{
// Local copy of SaveParameters that might be modified by checkSaveParameters( )
SaveParameters save_parameters_loc = save_parameters;
try {
// Checking map parameters for consistency
checkSaveParameters( save_parameters_loc );
tryWriteMapToFile( map, save_parameters_loc );
} catch ( std::exception & e ) {
std::cout << "[ERROR] [map_io]: Failed to write map for reason: " << e.what( ) << std::endl;
return false;
}
return true;
}
} // namespace nav2_map_server
1 // Copyright 2019 Rover Robotics
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "nav2_map_server/map_mode.hpp"
#include <stdexcept>
#include <string>
namespace nav2_map_server
{
22 const char * map_mode_to_string( MapMode map_mode )
{
switch ( map_mode ) {
case MapMode::Trinary:
return "trinary";
case MapMode::Scale:
return "scale";
case MapMode::Raw:
return "raw";
default:
throw std::invalid_argument( "map_mode" );
}
}
36 MapMode map_mode_from_string( std::string map_mode_name )
{
for ( auto & c : map_mode_name ) {
c = tolower( c );
}
if ( map_mode_name == "scale" ) {
return MapMode::Scale;
} else if ( map_mode_name == "raw" ) {
return MapMode::Raw;
} else if ( map_mode_name == "trinary" ) {
return MapMode::Trinary;
} else {
throw std::invalid_argument( "map_mode_name" );
}
}
} // namespace nav2_map_server
1 // Copyright ( c ) 2020 Samsung Research Russia
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include <stdexcept>
#include <string>
#include "nav2_map_server/map_saver.hpp"
#include "rclcpp/rclcpp.hpp"
23 int main( int argc, char ** argv )
{
rclcpp::init( argc, argv );
auto logger = rclcpp::get_logger( "map_saver_server" );
auto service_node = std::make_shared<nav2_map_server::MapSaver>( );
rclcpp::spin( service_node->get_node_base_interface( ) );
rclcpp::shutdown( );
return 0;
}
1 /*
* Copyright ( c ) 2020 Samsung Research Russia
* Copyright 2019 Rover Robotics
* Copyright ( c ) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the <ORGANIZATION> nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES ( INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE )
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "nav2_map_server/map_saver.hpp"
#include <string>
#include <memory>
#include <stdexcept>
#include <functional>
#include <mutex>
using namespace std::placeholders;
namespace nav2_map_server
{
44 MapSaver::MapSaver( const rclcpp::NodeOptions & options )
: nav2_util::LifecycleNode( "map_saver", "", options )
{
RCLCPP_INFO( get_logger( ), "Creating" );
// Declare the node parameters
declare_parameter( "save_map_timeout", 2.0 );
declare_parameter( "free_thresh_default", 0.25 );
declare_parameter( "occupied_thresh_default", 0.65 );
declare_parameter( "map_subscribe_transient_local", true );
}
56 MapSaver::~MapSaver( )
{
}
nav2_util::CallbackReturn
61 MapSaver::on_configure( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Configuring" );
// Make name prefix for services
const std::string service_prefix = get_name( ) + std::string( "/" );
save_map_timeout_ = std::make_shared<rclcpp::Duration>(
rclcpp::Duration::from_seconds( get_parameter( "save_map_timeout" ).as_double( ) ) );
free_thresh_default_ = get_parameter( "free_thresh_default" ).as_double( );
occupied_thresh_default_ = get_parameter( "occupied_thresh_default" ).as_double( );
map_subscribe_transient_local_ = get_parameter( "map_subscribe_transient_local" ).as_bool( );
// Create a service that saves the occupancy grid from map topic to a file
save_map_service_ = create_service<nav2_msgs::srv::SaveMap>(
service_prefix + save_map_service_name_,
std::bind( &MapSaver::saveMapCallback, this, _1, _2, _3 ) );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
83 MapSaver::on_activate( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Activating" );
// create bond connection
createBond( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
94 MapSaver::on_deactivate( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Deactivating" );
// destroy bond connection
destroyBond( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
105 MapSaver::on_cleanup( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Cleaning up" );
save_map_service_.reset( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
115 MapSaver::on_shutdown( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Shutting down" );
return nav2_util::CallbackReturn::SUCCESS;
}
121 void MapSaver::saveMapCallback(
122 const std::shared_ptr<rmw_request_id_t>/*request_header*/,
123 const std::shared_ptr<nav2_msgs::srv::SaveMap::Request> request,
124 std::shared_ptr<nav2_msgs::srv::SaveMap::Response> response )
{
// Set input arguments and call saveMapTopicToFile( )
SaveParameters save_parameters;
save_parameters.map_file_name = request->map_url;
save_parameters.image_format = request->image_format;
save_parameters.free_thresh = request->free_thresh;
save_parameters.occupied_thresh = request->occupied_thresh;
try {
save_parameters.mode = map_mode_from_string( request->map_mode );
} catch ( std::invalid_argument & ) {
save_parameters.mode = MapMode::Trinary;
RCLCPP_WARN(
get_logger( ), "Map mode parameter not recognized: '%s', using default value ( trinary )",
request->map_mode.c_str( ) );
}
response->result = saveMapTopicToFile( request->map_topic, save_parameters );
}
144 bool MapSaver::saveMapTopicToFile(
145 const std::string & map_topic,
146 const SaveParameters & save_parameters )
{
// Local copies of map_topic and save_parameters that could be changed
std::string map_topic_loc = map_topic;
SaveParameters save_parameters_loc = save_parameters;
RCLCPP_INFO(
get_logger( ), "Saving map from \'%s\' topic to \'%s\' file",
map_topic_loc.c_str( ), save_parameters_loc.map_file_name.c_str( ) );
try {
// Correct map_topic_loc if necessary
if ( map_topic_loc == "" ) {
map_topic_loc = "map";
RCLCPP_WARN(
get_logger( ), "Map topic unspecified. Map messages will be read from \'%s\' topic",
map_topic_loc.c_str( ) );
}
// Set default for MapSaver node thresholds parameters
if ( save_parameters_loc.free_thresh == 0.0 ) {
RCLCPP_WARN(
get_logger( ),
"Free threshold unspecified. Setting it to default value: %f",
free_thresh_default_ );
save_parameters_loc.free_thresh = free_thresh_default_;
}
if ( save_parameters_loc.occupied_thresh == 0.0 ) {
RCLCPP_WARN(
get_logger( ),
"Occupied threshold unspecified. Setting it to default value: %f",
occupied_thresh_default_ );
save_parameters_loc.occupied_thresh = occupied_thresh_default_;
}
std::promise<nav_msgs::msg::OccupancyGrid::SharedPtr> prom;
std::future<nav_msgs::msg::OccupancyGrid::SharedPtr> future_result = prom.get_future( );
// A callback function that receives map message from subscribed topic
auto mapCallback = [&prom](
const nav_msgs::msg::OccupancyGrid::SharedPtr msg ) -> void {
prom.set_value( msg );
};
rclcpp::QoS map_qos( 10 ); // initialize to default
if ( map_subscribe_transient_local_ ) {
map_qos.transient_local( );
map_qos.reliable( );
map_qos.keep_last( 1 );
}
// Create new CallbackGroup for map_sub
auto callback_group = create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive,
false );
auto option = rclcpp::SubscriptionOptions( );
option.callback_group = callback_group;
auto map_sub = create_subscription<nav_msgs::msg::OccupancyGrid>(
map_topic_loc, map_qos, mapCallback, option );
// Create SingleThreadedExecutor to spin map_sub in callback_group
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_callback_group( callback_group, get_node_base_interface( ) );
// Spin until map message received
auto timeout = save_map_timeout_->to_chrono<std::chrono::nanoseconds>( );
auto status = executor.spin_until_future_complete( future_result, timeout );
if ( status != rclcpp::FutureReturnCode::SUCCESS ) {
RCLCPP_ERROR( get_logger( ), "Failed to spin map subscription" );
return false;
}
// map_sub is no more needed
map_sub.reset( );
// Map message received. Saving it to file
nav_msgs::msg::OccupancyGrid::SharedPtr map_msg = future_result.get( );
if ( saveMapToFile( *map_msg, save_parameters_loc ) ) {
RCLCPP_INFO( get_logger( ), "Map saved successfully" );
return true;
} else {
RCLCPP_ERROR( get_logger( ), "Failed to save the map" );
return false;
}
} catch ( std::exception & e ) {
RCLCPP_ERROR( get_logger( ), "Failed to save the map: %s", e.what( ) );
return false;
}
return false;
}
} // namespace nav2_map_server
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
242 RCLCPP_COMPONENTS_REGISTER_NODE( nav2_map_server::MapSaver )
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include <stdexcept>
#include <string>
#include "nav2_map_server/map_server.hpp"
#include "rclcpp/rclcpp.hpp"
22 int main( int argc, char ** argv )
{
std::string node_name( "map_server" );
rclcpp::init( argc, argv );
auto node = std::make_shared<nav2_map_server::MapServer>( );
rclcpp::spin( node->get_node_base_interface( ) );
rclcpp::shutdown( );
}
1 /* Copyright ( c ) 2018 Intel Corporation
*
* Licensed under the Apache License, Version 2.0 ( the "License" );
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
/* Copyright 2019 Rover Robotics
* Copyright 2010 Brian Gerkey
* Copyright ( c ) 2008, Willow Garage, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES ( INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE )
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "nav2_map_server/map_server.hpp"
#include <string>
#include <memory>
#include <fstream>
#include <stdexcept>
#include <utility>
#include "yaml-cpp/yaml.h"
#include "lifecycle_msgs/msg/state.hpp"
#include "nav2_map_server/map_io.hpp"
using namespace std::chrono_literals;
using namespace std::placeholders;
namespace nav2_map_server
{
65 MapServer::MapServer( const rclcpp::NodeOptions & options )
: nav2_util::LifecycleNode( "map_server", "", options ), map_available_( false )
{
RCLCPP_INFO( get_logger( ), "Creating" );
// Declare the node parameters
declare_parameter( "yaml_filename", rclcpp::PARAMETER_STRING );
declare_parameter( "topic_name", "map" );
declare_parameter( "frame_id", "map" );
}
76 MapServer::~MapServer( )
{
}
nav2_util::CallbackReturn
81 MapServer::on_configure( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Configuring" );
// Get the name of the YAML file to use ( can be empty if no initial map should be used )
std::string yaml_filename = get_parameter( "yaml_filename" ).as_string( );
std::string topic_name = get_parameter( "topic_name" ).as_string( );
frame_id_ = get_parameter( "frame_id" ).as_string( );
// only try to load map if parameter was set
if ( !yaml_filename.empty( ) ) {
// Shared pointer to LoadMap::Response is also should be initialized
// in order to avoid null-pointer dereference
std::shared_ptr<nav2_msgs::srv::LoadMap::Response> rsp =
std::make_shared<nav2_msgs::srv::LoadMap::Response>( );
if ( !loadMapResponseFromYaml( yaml_filename, rsp ) ) {
throw std::runtime_error( "Failed to load map yaml file: " + yaml_filename );
}
} else {
RCLCPP_INFO(
get_logger( ),
"yaml-filename parameter is empty, set map through '%s'-service",
load_map_service_name_.c_str( ) );
}
// Make name prefix for services
const std::string service_prefix = get_name( ) + std::string( "/" );
// Create a service that provides the occupancy grid
occ_service_ = create_service<nav_msgs::srv::GetMap>(
service_prefix + std::string( service_name_ ),
std::bind( &MapServer::getMapCallback, this, _1, _2, _3 ) );
// Create a publisher using the QoS settings to emulate a ROS1 latched topic
occ_pub_ = create_publisher<nav_msgs::msg::OccupancyGrid>(
topic_name,
rclcpp::QoS( rclcpp::KeepLast( 1 ) ).transient_local( ).reliable( ) );
// Create a service that loads the occupancy grid from a file
load_map_service_ = create_service<nav2_msgs::srv::LoadMap>(
service_prefix + std::string( load_map_service_name_ ),
std::bind( &MapServer::loadMapCallback, this, _1, _2, _3 ) );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
129 MapServer::on_activate( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Activating" );
// Publish the map using the latched topic
occ_pub_->on_activate( );
if ( map_available_ ) {
auto occ_grid = std::make_unique<nav_msgs::msg::OccupancyGrid>( msg_ );
occ_pub_->publish( std::move( occ_grid ) );
}
// create bond connection
createBond( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
147 MapServer::on_deactivate( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Deactivating" );
occ_pub_->on_deactivate( );
// destroy bond connection
destroyBond( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
160 MapServer::on_cleanup( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Cleaning up" );
occ_pub_.reset( );
occ_service_.reset( );
load_map_service_.reset( );
map_available_ = false;
msg_ = nav_msgs::msg::OccupancyGrid( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
174 MapServer::on_shutdown( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Shutting down" );
return nav2_util::CallbackReturn::SUCCESS;
}
180 void MapServer::getMapCallback(
181 const std::shared_ptr<rmw_request_id_t>/*request_header*/,
182 const std::shared_ptr<nav_msgs::srv::GetMap::Request>/*request*/,
183 std::shared_ptr<nav_msgs::srv::GetMap::Response> response )
{
// if not in ACTIVE state, ignore request
if ( get_current_state( ).id( ) != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE ) {
RCLCPP_WARN(
get_logger( ),
"Received GetMap request but not in ACTIVE state, ignoring!" );
return;
}
RCLCPP_INFO( get_logger( ), "Handling GetMap request" );
response->map = msg_;
}
196 void MapServer::loadMapCallback(
197 const std::shared_ptr<rmw_request_id_t>/*request_header*/,
198 const std::shared_ptr<nav2_msgs::srv::LoadMap::Request> request,
199 std::shared_ptr<nav2_msgs::srv::LoadMap::Response> response )
{
// if not in ACTIVE state, ignore request
if ( get_current_state( ).id( ) != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE ) {
RCLCPP_WARN(
get_logger( ),
"Received LoadMap request but not in ACTIVE state, ignoring!" );
response->result = response->RESULT_UNDEFINED_FAILURE;
return;
}
RCLCPP_INFO( get_logger( ), "Handling LoadMap request" );
// Load from file
if ( loadMapResponseFromYaml( request->map_url, response ) ) {
auto occ_grid = std::make_unique<nav_msgs::msg::OccupancyGrid>( msg_ );
occ_pub_->publish( std::move( occ_grid ) ); // publish new map
}
}
217 bool MapServer::loadMapResponseFromYaml(
218 const std::string & yaml_file,
219 std::shared_ptr<nav2_msgs::srv::LoadMap::Response> response )
{
switch ( loadMapFromYaml( yaml_file, msg_ ) ) {
case MAP_DOES_NOT_EXIST:
response->result = nav2_msgs::srv::LoadMap::Response::RESULT_MAP_DOES_NOT_EXIST;
return false;
case INVALID_MAP_METADATA:
response->result = nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_METADATA;
return false;
case INVALID_MAP_DATA:
response->result = nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_DATA;
return false;
case LOAD_MAP_SUCCESS:
// Correcting msg_ header when it belongs to specific node
updateMsgHeader( );
map_available_ = true;
response->map = msg_;
response->result = nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS;
}
return true;
}
243 void MapServer::updateMsgHeader( )
{
msg_.info.map_load_time = now( );
msg_.header.frame_id = frame_id_;
msg_.header.stamp = now( );
}
} // namespace nav2_map_server
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
257 RCLCPP_COMPONENTS_REGISTER_NODE( nav2_map_server::MapServer )
// Copyright ( c ) 2020 Samsung Research Russia
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <string>
#include <memory>
#include <experimental/filesystem> // NOLINT
#include "rclcpp/rclcpp.hpp"
#include "test_constants/test_constants.h"
#include "nav2_map_server/map_saver.hpp"
#include "nav2_util/lifecycle_service_client.hpp"
#include "nav2_msgs/srv/save_map.hpp"
#define TEST_DIR TEST_DIRECTORY
using std::experimental::filesystem::path;
using lifecycle_msgs::msg::Transition;
using namespace nav2_map_server; // NOLINT
36 class RclCppFixture
{
public:
39 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
40 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
45 class MapSaverTestFixture : public ::testing::Test
{
public:
48 static void SetUpTestCase( )
{
node_ = rclcpp::Node::make_shared( "map_client_test" );
lifecycle_client_ =
std::make_shared<nav2_util::LifecycleServiceClient>( "map_saver", node_ );
RCLCPP_INFO( node_->get_logger( ), "Creating Test Node" );
std::this_thread::sleep_for( std::chrono::seconds( 5 ) ); // allow node to start up
const std::chrono::seconds timeout( 5 );
lifecycle_client_->change_state( Transition::TRANSITION_CONFIGURE, timeout );
lifecycle_client_->change_state( Transition::TRANSITION_ACTIVATE, timeout );
}
61 static void TearDownTestCase( )
{
lifecycle_client_->change_state( Transition::TRANSITION_DEACTIVATE );
lifecycle_client_->change_state( Transition::TRANSITION_CLEANUP );
lifecycle_client_.reset( );
node_.reset( );
}
template<class T>
typename T::Response::SharedPtr send_request(
rclcpp::Node::SharedPtr node,
typename rclcpp::Client<T>::SharedPtr client,
typename T::Request::SharedPtr request )
{
auto result = client->async_send_request( request );
// Wait for the result
if ( rclcpp::spin_until_future_complete( node, result ) == rclcpp::FutureReturnCode::SUCCESS ) {
return result.get( );
} else {
return nullptr;
}
}
protected:
// Check that map_msg corresponds to reference pattern
// Input: map_msg
void verifyMapMsg( const nav_msgs::msg::OccupancyGrid & map_msg )
{
ASSERT_FLOAT_EQ( map_msg.info.resolution, g_valid_image_res );
ASSERT_EQ( map_msg.info.width, g_valid_image_width );
ASSERT_EQ( map_msg.info.height, g_valid_image_height );
for ( unsigned int i = 0; i < map_msg.info.width * map_msg.info.height; i++ ) {
ASSERT_EQ( g_valid_image_content[i], map_msg.data[i] );
}
}
static rclcpp::Node::SharedPtr node_;
static std::shared_ptr<nav2_util::LifecycleServiceClient> lifecycle_client_;
};
rclcpp::Node::SharedPtr MapSaverTestFixture::node_ = nullptr;
std::shared_ptr<nav2_util::LifecycleServiceClient> MapSaverTestFixture::lifecycle_client_ =
nullptr;
// Send map saving service request.
// Load saved map and verify obtained OccupancyGrid.
TEST_F( MapSaverTestFixture, SaveMap )
{
RCLCPP_INFO( node_->get_logger( ), "Testing SaveMap service" );
auto req = std::make_shared<nav2_msgs::srv::SaveMap::Request>( );
auto client = node_->create_client<nav2_msgs::srv::SaveMap>(
"/map_saver/save_map" );
RCLCPP_INFO( node_->get_logger( ), "Waiting for save_map service" );
ASSERT_TRUE( client->wait_for_service( ) );
// 1. Send valid save_map serivce request
req->map_topic = "map";
req->map_url = path( g_tmp_dir ) / path( g_valid_map_name );
req->image_format = "png";
req->map_mode = "trinary";
req->free_thresh = g_default_free_thresh;
req->occupied_thresh = g_default_occupied_thresh;
auto resp = send_request<nav2_msgs::srv::SaveMap>( node_, client, req );
ASSERT_EQ( resp->result, true );
// 2. Load saved map and verify it
nav_msgs::msg::OccupancyGrid map_msg;
LOAD_MAP_STATUS status = loadMapFromYaml( path( g_tmp_dir ) / path( g_valid_yaml_file ), map_msg );
ASSERT_EQ( status, LOAD_MAP_SUCCESS );
verifyMapMsg( map_msg );
}
// Send map saving service request with default parameters.
// Load saved map and verify obtained OccupancyGrid.
TEST_F( MapSaverTestFixture, SaveMapDefaultParameters )
{
RCLCPP_INFO( node_->get_logger( ), "Testing SaveMap service" );
auto req = std::make_shared<nav2_msgs::srv::SaveMap::Request>( );
auto client = node_->create_client<nav2_msgs::srv::SaveMap>(
"/map_saver/save_map" );
RCLCPP_INFO( node_->get_logger( ), "Waiting for save_map service" );
ASSERT_TRUE( client->wait_for_service( ) );
// 1. Send save_map serivce request with default parameters
req->map_topic = "";
req->map_url = path( g_tmp_dir ) / path( g_valid_map_name );
req->image_format = "";
req->map_mode = "";
req->free_thresh = 0.0;
req->occupied_thresh = 0.0;
auto resp = send_request<nav2_msgs::srv::SaveMap>( node_, client, req );
ASSERT_EQ( resp->result, true );
// 2. Load saved map and verify it
nav_msgs::msg::OccupancyGrid map_msg;
LOAD_MAP_STATUS status = loadMapFromYaml( path( g_tmp_dir ) / path( g_valid_yaml_file ), map_msg );
ASSERT_EQ( status, LOAD_MAP_SUCCESS );
verifyMapMsg( map_msg );
}
// Send map saving service requests with different sets of parameters.
// In case of map is expected to be saved correctly, load map from a saved
// file and verify obtained OccupancyGrid.
TEST_F( MapSaverTestFixture, SaveMapInvalidParameters )
{
RCLCPP_INFO( node_->get_logger( ), "Testing SaveMap service" );
auto req = std::make_shared<nav2_msgs::srv::SaveMap::Request>( );
auto client = node_->create_client<nav2_msgs::srv::SaveMap>(
"/map_saver/save_map" );
RCLCPP_INFO( node_->get_logger( ), "Waiting for save_map service" );
ASSERT_TRUE( client->wait_for_service( ) );
// 1. Trying to send save_map serivce request with different sets of parameters
// In case of map is expected to be saved correctly, verify it
req->map_topic = "invalid_map";
req->map_url = path( g_tmp_dir ) / path( g_valid_map_name );
req->image_format = "png";
req->map_mode = "trinary";
req->free_thresh = g_default_free_thresh;
req->occupied_thresh = g_default_occupied_thresh;
auto resp = send_request<nav2_msgs::srv::SaveMap>( node_, client, req );
ASSERT_EQ( resp->result, false );
req->map_topic = "map";
req->image_format = "invalid_format";
resp = send_request<nav2_msgs::srv::SaveMap>( node_, client, req );
ASSERT_EQ( resp->result, true );
nav_msgs::msg::OccupancyGrid map_msg;
LOAD_MAP_STATUS status = loadMapFromYaml( path( g_tmp_dir ) / path( g_valid_yaml_file ), map_msg );
ASSERT_EQ( status, LOAD_MAP_SUCCESS );
verifyMapMsg( map_msg );
req->image_format = "png";
req->map_mode = "invalid_mode";
resp = send_request<nav2_msgs::srv::SaveMap>( node_, client, req );
ASSERT_EQ( resp->result, true );
status = loadMapFromYaml( path( g_tmp_dir ) / path( g_valid_yaml_file ), map_msg );
ASSERT_EQ( status, LOAD_MAP_SUCCESS );
verifyMapMsg( map_msg );
req->map_mode = "trinary";
req->free_thresh = 2.0;
req->occupied_thresh = 2.0;
resp = send_request<nav2_msgs::srv::SaveMap>( node_, client, req );
ASSERT_EQ( resp->result, false );
req->free_thresh = -2.0;
req->occupied_thresh = -2.0;
resp = send_request<nav2_msgs::srv::SaveMap>( node_, client, req );
ASSERT_EQ( resp->result, false );
req->free_thresh = 0.7;
req->occupied_thresh = 0.2;
resp = send_request<nav2_msgs::srv::SaveMap>( node_, client, req );
ASSERT_EQ( resp->result, false );
}
1 // Copyright ( c ) 2020 Samsung Research Russia
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <experimental/filesystem>
#include <string>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "nav2_map_server/map_io.hpp"
#include "test_constants/test_constants.h"
#define TEST_DIR TEST_DIRECTORY
using namespace nav2_map_server; // NOLINT
using std::experimental::filesystem::path;
28 class TestPublisher : public rclcpp::Node
{
public:
31 TestPublisher( )
: Node( "map_publisher" )
{
std::string pub_map_file = path( TEST_DIR ) / path( g_valid_yaml_file );
nav_msgs::msg::OccupancyGrid msg;
LOAD_MAP_STATUS status = loadMapFromYaml( pub_map_file, msg );
if ( status != LOAD_MAP_SUCCESS ) {
RCLCPP_ERROR( get_logger( ), "Can not load %s map file", pub_map_file.c_str( ) );
return;
}
map_pub_ = create_publisher<nav_msgs::msg::OccupancyGrid>(
"map",
rclcpp::QoS( rclcpp::KeepLast( 1 ) ).transient_local( ).reliable( ) );
map_pub_->publish( msg );
}
protected:
49 rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_pub_;
};
52 int main( int argc, char ** argv )
{
rclcpp::init( argc, argv );
auto pub_node = std::make_shared<TestPublisher>( );
rclcpp::spin( pub_node );
rclcpp::shutdown( );
return 0;
}
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <string>
#include <memory>
#include <experimental/filesystem> // NOLINT
#include <rclcpp/rclcpp.hpp>
#include "test_constants/test_constants.h"
#include "nav2_map_server/map_server.hpp"
#include "nav2_util/lifecycle_service_client.hpp"
#include "nav2_msgs/srv/load_map.hpp"
using namespace std::chrono_literals;
using namespace rclcpp; // NOLINT
#define TEST_DIR TEST_DIRECTORY
using std::experimental::filesystem::path;
using lifecycle_msgs::msg::Transition;
36 class RclCppFixture
{
public:
39 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
40 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
45 class MapServerTestFixture : public ::testing::Test
{
public:
48 static void SetUpTestCase( )
{
node_ = rclcpp::Node::make_shared( "map_client_test" );
lifecycle_client_ =
std::make_shared<nav2_util::LifecycleServiceClient>( "map_server", node_ );
RCLCPP_INFO( node_->get_logger( ), "Creating Test Node" );
std::this_thread::sleep_for( std::chrono::seconds( 5 ) ); // allow node to start up
const std::chrono::seconds timeout( 5 );
lifecycle_client_->change_state( Transition::TRANSITION_CONFIGURE, timeout );
lifecycle_client_->change_state( Transition::TRANSITION_ACTIVATE, timeout );
}
61 static void TearDownTestCase( )
{
lifecycle_client_->change_state( Transition::TRANSITION_DEACTIVATE );
lifecycle_client_->change_state( Transition::TRANSITION_CLEANUP );
lifecycle_client_->change_state( Transition::TRANSITION_UNCONFIGURED_SHUTDOWN );
lifecycle_client_.reset( );
node_.reset( );
}
template<class T>
typename T::Response::SharedPtr send_request(
rclcpp::Node::SharedPtr node,
typename rclcpp::Client<T>::SharedPtr client,
typename T::Request::SharedPtr request )
{
auto result = client->async_send_request( request );
// Wait for the result
if ( rclcpp::spin_until_future_complete( node, result ) == rclcpp::FutureReturnCode::SUCCESS ) {
return result.get( );
} else {
return nullptr;
}
}
protected:
// Check that map_msg corresponds to reference pattern
// Input: map_msg
void verifyMapMsg( const nav_msgs::msg::OccupancyGrid & map_msg )
{
ASSERT_FLOAT_EQ( map_msg.info.resolution, g_valid_image_res );
ASSERT_EQ( map_msg.info.width, g_valid_image_width );
ASSERT_EQ( map_msg.info.height, g_valid_image_height );
for ( unsigned int i = 0; i < map_msg.info.width * map_msg.info.height; i++ ) {
ASSERT_EQ( g_valid_image_content[i], map_msg.data[i] );
}
}
static rclcpp::Node::SharedPtr node_;
static std::shared_ptr<nav2_util::LifecycleServiceClient> lifecycle_client_;
};
rclcpp::Node::SharedPtr MapServerTestFixture::node_ = nullptr;
std::shared_ptr<nav2_util::LifecycleServiceClient> MapServerTestFixture::lifecycle_client_ =
nullptr;
// Send map getting service request and verify obtained OccupancyGrid
TEST_F( MapServerTestFixture, GetMap )
{
RCLCPP_INFO( node_->get_logger( ), "Testing GetMap service" );
auto req = std::make_shared<nav_msgs::srv::GetMap::Request>( );
auto client = node_->create_client<nav_msgs::srv::GetMap>(
"/map_server/map" );
RCLCPP_INFO( node_->get_logger( ), "Waiting for map service" );
ASSERT_TRUE( client->wait_for_service( ) );
auto resp = send_request<nav_msgs::srv::GetMap>( node_, client, req );
verifyMapMsg( resp->map );
}
// Send map loading service request and verify obtained OccupancyGrid
TEST_F( MapServerTestFixture, LoadMap )
{
RCLCPP_INFO( node_->get_logger( ), "Testing LoadMap service" );
auto req = std::make_shared<nav2_msgs::srv::LoadMap::Request>( );
auto client = node_->create_client<nav2_msgs::srv::LoadMap>(
"/map_server/load_map" );
RCLCPP_INFO( node_->get_logger( ), "Waiting for load_map service" );
ASSERT_TRUE( client->wait_for_service( ) );
req->map_url = path( TEST_DIR ) / path( g_valid_yaml_file );
auto resp = send_request<nav2_msgs::srv::LoadMap>( node_, client, req );
ASSERT_EQ( resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS );
verifyMapMsg( resp->map );
}
// Send map loading service request without specifying which map to load
TEST_F( MapServerTestFixture, LoadMapNull )
{
RCLCPP_INFO( node_->get_logger( ), "Testing LoadMap service" );
auto req = std::make_shared<nav2_msgs::srv::LoadMap::Request>( );
auto client = node_->create_client<nav2_msgs::srv::LoadMap>(
"/map_server/load_map" );
RCLCPP_INFO( node_->get_logger( ), "Waiting for load_map service" );
ASSERT_TRUE( client->wait_for_service( ) );
req->map_url = "";
RCLCPP_INFO( node_->get_logger( ), "Sending load_map request with null file name" );
auto resp = send_request<nav2_msgs::srv::LoadMap>( node_, client, req );
ASSERT_EQ( resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_MAP_DOES_NOT_EXIST );
}
// Send map loading service request with non-existing yaml file
TEST_F( MapServerTestFixture, LoadMapInvalidYaml )
{
RCLCPP_INFO( node_->get_logger( ), "Testing LoadMap service" );
auto req = std::make_shared<nav2_msgs::srv::LoadMap::Request>( );
auto client = node_->create_client<nav2_msgs::srv::LoadMap>(
"/map_server/load_map" );
RCLCPP_INFO( node_->get_logger( ), "Waiting for load_map service" );
ASSERT_TRUE( client->wait_for_service( ) );
req->map_url = "invalid_file.yaml";
RCLCPP_INFO( node_->get_logger( ), "Sending load_map request with invalid yaml file name" );
auto resp = send_request<nav2_msgs::srv::LoadMap>( node_, client, req );
ASSERT_EQ( resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_METADATA );
}
// Send map loading service request with yaml file containing non-existing map
TEST_F( MapServerTestFixture, LoadMapInvalidImage )
{
RCLCPP_INFO( node_->get_logger( ), "Testing LoadMap service" );
auto req = std::make_shared<nav2_msgs::srv::LoadMap::Request>( );
auto client = node_->create_client<nav2_msgs::srv::LoadMap>(
"/map_server/load_map" );
RCLCPP_INFO( node_->get_logger( ), "Waiting for load_map service" );
ASSERT_TRUE( client->wait_for_service( ) );
req->map_url = path( TEST_DIR ) / "invalid_image.yaml";
RCLCPP_INFO( node_->get_logger( ), "Sending load_map request with invalid image file name" );
auto resp = send_request<nav2_msgs::srv::LoadMap>( node_, client, req );
ASSERT_EQ( resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_DATA );
}
/**
* Test behaviour of server if yaml_filename is set to an empty string.
*/
TEST_F( MapServerTestFixture, NoInitialMap )
{
// turn off node into unconfigured state
lifecycle_client_->change_state( Transition::TRANSITION_DEACTIVATE );
lifecycle_client_->change_state( Transition::TRANSITION_CLEANUP );
auto client = node_->create_client<nav_msgs::srv::GetMap>( "/map_server/map" );
auto req = std::make_shared<nav_msgs::srv::GetMap::Request>( );
auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>( node_, "map_server" );
ASSERT_TRUE( parameters_client->wait_for_service( 3s ) );
// set yaml_filename-parameter to empty string ( essentially restart the node )
RCLCPP_INFO( node_->get_logger( ), "Removing yaml_filename-parameter before restarting" );
parameters_client->set_parameters( {Parameter( "yaml_filename", ParameterValue( "" ) )} );
// only configure node, to test behaviour of service while node is not active
lifecycle_client_->change_state( Transition::TRANSITION_CONFIGURE, 3s );
RCLCPP_INFO( node_->get_logger( ), "Testing LoadMap service while not being active" );
auto load_map_req = std::make_shared<nav2_msgs::srv::LoadMap::Request>( );
auto load_map_cl = node_->create_client<nav2_msgs::srv::LoadMap>( "/map_server/load_map" );
ASSERT_TRUE( load_map_cl->wait_for_service( 3s ) );
auto resp = send_request<nav2_msgs::srv::LoadMap>( node_, load_map_cl, load_map_req );
ASSERT_EQ( resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_UNDEFINED_FAILURE );
// activate server and load map:
lifecycle_client_->change_state( Transition::TRANSITION_ACTIVATE, 3s );
RCLCPP_INFO( node_->get_logger( ), "active again" );
load_map_req->map_url = path( TEST_DIR ) / path( g_valid_yaml_file );
auto load_res = send_request<nav2_msgs::srv::LoadMap>( node_, load_map_cl, load_map_req );
ASSERT_EQ( load_res->result, nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS );
verifyMapMsg( load_res->map );
}
1 // Copyright ( c ) 2020 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <experimental/filesystem>
#include <string>
#include <memory>
#include <utility>
#include "rclcpp/rclcpp.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
24 TEST( MapSaverCLI, CLITest )
{
std::string path = "/tmp/";
std::string file = "test_map";
std::string file_path = path + file;
rclcpp::init( 0, nullptr );
auto node = std::make_shared<rclcpp::Node>( "CLI_Test_Node" );
RCLCPP_INFO( node->get_logger( ), "Testing Map Saver CLI" );
auto publisher = node->create_publisher<nav_msgs::msg::OccupancyGrid>(
"/map",
rclcpp::QoS( rclcpp::KeepLast( 1 ) ).transient_local( ).reliable( ) );
auto msg = std::make_unique<nav_msgs::msg::OccupancyGrid>( );
msg->header.frame_id = "map";
msg->header.stamp = node->now( );
msg->info.map_load_time = node->now( );
msg->info.resolution = 0.05;
msg->info.width = 3;
msg->info.height = 3;
msg->info.origin.position.x = 0.0;
msg->info.origin.position.y = 0.0;
msg->info.origin.orientation.w = 1.0;
msg->data.resize( 9 );
msg->data[0] = 0;
msg->data[2] = 100;
msg->data[1] = 101;
msg->data[3] = 50;
RCLCPP_INFO( node->get_logger( ), "Publishing occupancy grid..." );
publisher->publish( std::move( msg ) );
rclcpp::Rate( 1 ).sleep( );
// succeed on real map
RCLCPP_INFO( node->get_logger( ), "Calling saver..." );
EXPECT_FALSE( std::experimental::filesystem::exists( file_path + ".yaml" ) );
std::string command =
std::string(
"ros2 run nav2_map_server map_saver_cli -f " ) + file_path;
auto return_code = system( command.c_str( ) );
EXPECT_EQ( return_code, 0 );
rclcpp::Rate( 0.25 ).sleep( );
RCLCPP_INFO( node->get_logger( ), "Checking on file..." );
EXPECT_TRUE( std::experimental::filesystem::exists( file_path + ".pgm" ) );
EXPECT_EQ( std::experimental::filesystem::file_size( file_path + ".pgm" ), 20ul );
if ( std::experimental::filesystem::exists( file_path + ".yaml" ) ) {
std::experimental::filesystem::remove( file_path + ".yaml" );
}
if ( std::experimental::filesystem::exists( file_path + ".pgm" ) ) {
std::experimental::filesystem::remove( file_path + ".pgm" );
}
// fail on bogus map
RCLCPP_INFO( node->get_logger( ), "Calling saver..." );
EXPECT_FALSE( std::experimental::filesystem::exists( file_path + ".yaml" ) );
command =
std::string(
"ros2 run nav2_map_server map_saver_cli "
"-t map_failure --occ 100 --free 2 --mode trinary --fmt png -f " ) + file_path +
std::string( "--ros-args __node:=map_saver_test_node" );
return_code = system( command.c_str( ) );
EXPECT_EQ( return_code, 65280 );
rclcpp::Rate( 0.25 ).sleep( );
RCLCPP_INFO( node->get_logger( ), "Checking on file..." );
EXPECT_FALSE( std::experimental::filesystem::exists( file_path + ".yaml" ) );
RCLCPP_INFO( node->get_logger( ), "Testing help..." );
command =
std::string(
"ros2 run nav2_map_server map_saver_cli -h" );
return_code = system( command.c_str( ) );
EXPECT_EQ( return_code, 0 );
rclcpp::Rate( 0.5 ).sleep( );
RCLCPP_INFO( node->get_logger( ), "Testing invalid mode..." );
command =
std::string(
"ros2 run nav2_map_server map_saver_cli --mode fake_mode" );
return_code = system( command.c_str( ) );
EXPECT_EQ( return_code, 0 );
rclcpp::Rate( 0.5 ).sleep( );
RCLCPP_INFO( node->get_logger( ), "Testing missing argument..." );
command =
std::string(
"ros2 run nav2_map_server map_saver_cli --mode" );
return_code = system( command.c_str( ) );
EXPECT_EQ( return_code, 65280 );
rclcpp::Rate( 0.5 ).sleep( );
RCLCPP_INFO( node->get_logger( ), "Testing wrong argument..." );
command =
std::string(
"ros2 run nav2_map_server map_saver_cli --free 0 0" );
return_code = system( command.c_str( ) );
EXPECT_EQ( return_code, 65280 );
rclcpp::Rate( 0.5 ).sleep( );
command =
std::string(
"ros2 run nav2_map_server map_saver_cli --ros-args -r __node:=map_saver_test_node" );
return_code = system( command.c_str( ) );
EXPECT_EQ( return_code, 0 );
}
1 /*
* Copyright ( c ) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES ( INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE )
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef TEST_CONSTANTS__TEST_CONSTANTS_H_
#define TEST_CONSTANTS__TEST_CONSTANTS_H_
/* Author: Brian Gerkey */
/* This file externs global constants shared among tests */
#include <vector>
extern const unsigned int g_valid_image_width;
extern const unsigned int g_valid_image_height;
extern const char g_valid_image_content[];
extern const char * g_valid_map_name;
extern const char * g_valid_png_file;
extern const char * g_valid_bmp_file;
extern const char * g_valid_pgm_file;
extern const char * g_valid_yaml_file;
extern const char * g_tmp_dir;
extern const double g_valid_image_res;
// *INDENT-OFF*
// Uncrustify may incorrectly guide to add extra spaces in < double > during CI tests
extern const std::vector<double> g_valid_origin;
// *INDENT-ON*
extern const double g_default_free_thresh;
extern const double g_default_occupied_thresh;
#endif // TEST_CONSTANTS__TEST_CONSTANTS_H_
1 // Copyright ( c ) 2020 Samsung Research Russia
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <string>
#include <memory>
#include <chrono>
#include <limits>
#include <mutex>
#include "rclcpp/rclcpp.hpp"
#include "nav2_map_server/costmap_filter_info_server.hpp"
using namespace std::chrono_literals;
typedef std::recursive_mutex mutex_t;
static const char FILTER_INFO_TOPIC[] = "filter_info";
static const int TYPE = 1;
static const char MASK_TOPIC[] = "mask";
static const double BASE = 0.1;
static const double MULTIPLIER = 0.2;
static const double EPSILON = std::numeric_limits<float>::epsilon( );
39 class RclCppFixture
{
public:
42 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
43 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
47 class InfoServerWrapper : public nav2_map_server::CostmapFilterInfoServer
{
public:
50 void start( )
{
on_configure( get_current_state( ) );
on_activate( get_current_state( ) );
}
56 void stop( )
{
on_deactivate( get_current_state( ) );
on_cleanup( get_current_state( ) );
on_shutdown( get_current_state( ) );
}
};
64 class InfoServerTester : public ::testing::Test
{
public:
67 InfoServerTester( )
: info_server_( nullptr ), info_( nullptr ), subscription_( nullptr )
{
access_ = new mutex_t( );
info_server_ = std::make_shared<InfoServerWrapper>( );
try {
info_server_->set_parameter( rclcpp::Parameter( "filter_info_topic", FILTER_INFO_TOPIC ) );
info_server_->set_parameter( rclcpp::Parameter( "type", TYPE ) );
info_server_->set_parameter( rclcpp::Parameter( "mask_topic", MASK_TOPIC ) );
info_server_->set_parameter( rclcpp::Parameter( "base", BASE ) );
info_server_->set_parameter( rclcpp::Parameter( "multiplier", MULTIPLIER ) );
} catch ( rclcpp::exceptions::ParameterNotDeclaredException & ex ) {
RCLCPP_ERROR(
info_server_->get_logger( ),
"Error while setting parameters for CostmapFilterInfoServer: %s", ex.what( ) );
throw;
}
info_server_->start( );
subscription_ = info_server_->create_subscription<nav2_msgs::msg::CostmapFilterInfo>(
FILTER_INFO_TOPIC, rclcpp::QoS( rclcpp::KeepLast( 1 ) ).transient_local( ).reliable( ),
std::bind( &InfoServerTester::infoCallback, this, std::placeholders::_1 ) );
}
93 ~InfoServerTester( )
{
info_server_->stop( );
info_server_.reset( );
subscription_.reset( );
}
100 bool isReceived( )
{
std::lock_guard<mutex_t> guard( *getMutex( ) );
if ( info_ ) {
return true;
} else {
return false;
}
}
110 mutex_t * getMutex( )
{
return access_;
}
protected:
116 std::shared_ptr<InfoServerWrapper> info_server_;
117 nav2_msgs::msg::CostmapFilterInfo::SharedPtr info_;
private:
120 void infoCallback( const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg )
{
std::lock_guard<mutex_t> guard( *getMutex( ) );
info_ = msg;
}
126 rclcpp::Subscription<nav2_msgs::msg::CostmapFilterInfo>::SharedPtr subscription_;
128 mutex_t * access_;
};
131 TEST_F( InfoServerTester, testCostmapFilterInfoPublish )
{
rclcpp::Time start_time = info_server_->now( );
while ( !isReceived( ) ) {
rclcpp::spin_some( info_server_->get_node_base_interface( ) );
std::this_thread::sleep_for( 100ms );
// Waiting no more than 5 seconds
ASSERT_TRUE( ( info_server_->now( ) - start_time ) <= rclcpp::Duration( 5000ms ) );
}
// Checking received CostmapFilterInfo for consistency
EXPECT_EQ( info_->type, TYPE );
EXPECT_EQ( info_->filter_mask_topic, MASK_TOPIC );
EXPECT_NEAR( info_->base, BASE, EPSILON );
EXPECT_NEAR( info_->multiplier, MULTIPLIER, EPSILON );
}
1 // Copyright 2019 Rover Robotics
/*
* Copyright ( c ) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES ( INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE )
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/* Author: Brian Gerkey */
#include <gtest/gtest.h>
#include <experimental/filesystem>
#include <stdexcept>
#include <string>
#include <vector>
#include <memory>
#include <iostream>
#include <fstream>
#include "yaml-cpp/yaml.h"
#include "nav2_map_server/map_io.hpp"
#include "nav2_map_server/map_server.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "test_constants/test_constants.h"
#define TEST_DIR TEST_DIRECTORY
using namespace std; // NOLINT
using namespace nav2_map_server; // NOLINT
using std::experimental::filesystem::path;
55 class RclCppFixture
{
public:
58 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
59 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
64 class MapIOTester : public ::testing::Test
{
protected:
// Fill LoadParameters with standard for testing values
// Input: image_file_name
// Output: load_parameters
70 void fillLoadParameters(
71 const std::string & image_file_name,
LoadParameters & load_parameters )
{
load_parameters.image_file_name = image_file_name;
load_parameters.resolution = g_valid_image_res;
load_parameters.origin = g_valid_origin;
load_parameters.free_thresh = g_default_free_thresh;
load_parameters.occupied_thresh = g_default_occupied_thresh;
load_parameters.mode = MapMode::Trinary;
load_parameters.negate = 0;
}
// Fill SaveParameters with standard for testing values
// Input: map_file_name, image_format
// Output: save_parameters
86 void fillSaveParameters(
87 const std::string & map_file_name,
88 const std::string & image_format,
89 SaveParameters & save_parameters )
{
save_parameters.map_file_name = map_file_name;
save_parameters.image_format = image_format;
save_parameters.free_thresh = g_default_free_thresh;
save_parameters.occupied_thresh = g_default_occupied_thresh;
save_parameters.mode = MapMode::Trinary;
}
// Check that map_msg corresponds to reference pattern
// Input: map_msg
100 void verifyMapMsg( const nav_msgs::msg::OccupancyGrid & map_msg )
{
ASSERT_FLOAT_EQ( map_msg.info.resolution, g_valid_image_res );
ASSERT_EQ( map_msg.info.width, g_valid_image_width );
ASSERT_EQ( map_msg.info.height, g_valid_image_height );
for ( unsigned int i = 0; i < map_msg.info.width * map_msg.info.height; i++ ) {
ASSERT_EQ( g_valid_image_content[i], map_msg.data[i] );
}
}
};
// Load a valid reference PGM file. Check obtained OccupancyGrid message for consistency:
// loaded image should match the known dimensions and content of the file.
// Save obtained OccupancyGrid message into a tmp PGM file. Then load back saved tmp file
// and check for consistency.
// Succeeds all steps were passed without a problem or expection.
116 TEST_F( MapIOTester, loadSaveValidPGM )
{
// 1. Load reference map file and verify obtained OccupancyGrid
LoadParameters loadParameters;
fillLoadParameters( path( TEST_DIR ) / path( g_valid_pgm_file ), loadParameters );
nav_msgs::msg::OccupancyGrid map_msg;
ASSERT_NO_THROW( loadMapFromFile( loadParameters, map_msg ) );
verifyMapMsg( map_msg );
// 2. Save OccupancyGrid into a tmp file
SaveParameters saveParameters;
fillSaveParameters( path( g_tmp_dir ) / path( g_valid_map_name ), "pgm", saveParameters );
ASSERT_TRUE( saveMapToFile( map_msg, saveParameters ) );
// 3. Load saved map and verify it
LOAD_MAP_STATUS status = loadMapFromYaml( path( g_tmp_dir ) / path( g_valid_yaml_file ), map_msg );
ASSERT_EQ( status, LOAD_MAP_SUCCESS );
verifyMapMsg( map_msg );
}
// Load a valid reference PNG file. Check obtained OccupancyGrid message for consistency:
// loaded image should match the known dimensions and content of the file.
// Save obtained OccupancyGrid message into a tmp PNG file. Then load back saved tmp file
// and check for consistency.
// Succeeds all steps were passed without a problem or expection.
145 TEST_F( MapIOTester, loadSaveValidPNG )
{
// 1. Load reference map file and verify obtained OccupancyGrid
LoadParameters loadParameters;
fillLoadParameters( path( TEST_DIR ) / path( g_valid_png_file ), loadParameters );
nav_msgs::msg::OccupancyGrid map_msg;
ASSERT_NO_THROW( loadMapFromFile( loadParameters, map_msg ) );
verifyMapMsg( map_msg );
// 2. Save OccupancyGrid into a tmp file
SaveParameters saveParameters;
fillSaveParameters( path( g_tmp_dir ) / path( g_valid_map_name ), "png", saveParameters );
ASSERT_TRUE( saveMapToFile( map_msg, saveParameters ) );
// 3. Load saved map and verify it
LOAD_MAP_STATUS status = loadMapFromYaml( path( g_tmp_dir ) / path( g_valid_yaml_file ), map_msg );
ASSERT_EQ( status, LOAD_MAP_SUCCESS );
verifyMapMsg( map_msg );
}
// Load a valid reference BMP file. Check obtained OccupancyGrid message for consistency:
// loaded image should match the known dimensions and content of the file.
// Save obtained OccupancyGrid message into a tmp BMP file. Then load back saved tmp file
// and check for consistency.
// Succeeds all steps were passed without a problem or expection.
174 TEST_F( MapIOTester, loadSaveValidBMP )
{
// 1. Load reference map file and verify obtained OccupancyGrid
auto test_bmp = path( TEST_DIR ) / path( g_valid_bmp_file );
LoadParameters loadParameters;
fillLoadParameters( test_bmp, loadParameters );
nav_msgs::msg::OccupancyGrid map_msg;
ASSERT_NO_THROW( loadMapFromFile( loadParameters, map_msg ) );
verifyMapMsg( map_msg );
// 2. Save OccupancyGrid into a tmp file
SaveParameters saveParameters;
fillSaveParameters( path( g_tmp_dir ) / path( g_valid_map_name ), "bmp", saveParameters );
ASSERT_TRUE( saveMapToFile( map_msg, saveParameters ) );
// 3. Load saved map and verify it
LOAD_MAP_STATUS status = loadMapFromYaml( path( g_tmp_dir ) / path( g_valid_yaml_file ), map_msg );
ASSERT_EQ( status, LOAD_MAP_SUCCESS );
verifyMapMsg( map_msg );
}
// Load map from a valid file. Trying to save map with different modes.
// Succeeds all steps were passed without a problem or expection.
202 TEST_F( MapIOTester, loadSaveMapModes )
{
// 1. Load map from YAML file
nav_msgs::msg::OccupancyGrid map_msg;
LOAD_MAP_STATUS status = loadMapFromYaml( path( TEST_DIR ) / path( g_valid_yaml_file ), map_msg );
ASSERT_EQ( status, LOAD_MAP_SUCCESS );
// No need to check Trinary mode. This already verified in previous testcases.
// 2. Save map in Scale mode.
SaveParameters saveParameters;
fillSaveParameters( path( g_tmp_dir ) / path( g_valid_map_name ), "png", saveParameters );
saveParameters.mode = MapMode::Scale;
ASSERT_TRUE( saveMapToFile( map_msg, saveParameters ) );
// 3. Load saved map and verify it
status = loadMapFromYaml( path( g_tmp_dir ) / path( g_valid_yaml_file ), map_msg );
ASSERT_EQ( status, LOAD_MAP_SUCCESS );
verifyMapMsg( map_msg );
// 4. Save map in Raw mode.
saveParameters.mode = MapMode::Raw;
ASSERT_TRUE( saveMapToFile( map_msg, saveParameters ) );
// 5. Load saved map and verify it
status = loadMapFromYaml( path( g_tmp_dir ) / path( g_valid_yaml_file ), map_msg );
ASSERT_EQ( status, LOAD_MAP_SUCCESS );
verifyMapMsg( map_msg );
}
// Try to load an invalid file with different ways.
// Succeeds if all cases are got expected fail behaviours.
237 TEST_F( MapIOTester, loadInvalidFile )
{
// 1. Trying to load incorrect map by loadMapFromFile( )
auto test_invalid = path( TEST_DIR ) / path( "foo" );
LoadParameters loadParameters;
fillLoadParameters( test_invalid, loadParameters );
nav_msgs::msg::OccupancyGrid map_msg;
ASSERT_ANY_THROW( loadMapFromFile( loadParameters, map_msg ) );
// 2. Trying to load incorrect map by loadMapFromYaml( )
LOAD_MAP_STATUS status = loadMapFromYaml( "", map_msg );
ASSERT_EQ( status, MAP_DOES_NOT_EXIST );
status = loadMapFromYaml( std::string( test_invalid ) + ".yaml", map_msg );
ASSERT_EQ( status, INVALID_MAP_METADATA );
}
// Load map from a valid file. Trying to save map with different sets of parameters.
// Succeeds if all cases got expected behaviours.
258 TEST_F( MapIOTester, saveInvalidParameters )
{
// 1. Load map from YAML file
nav_msgs::msg::OccupancyGrid map_msg;
LOAD_MAP_STATUS status = loadMapFromYaml( path( TEST_DIR ) / path( g_valid_yaml_file ), map_msg );
ASSERT_EQ( status, LOAD_MAP_SUCCESS );
// 2. Trying to save map with different sets of parameters
SaveParameters saveParameters;
saveParameters.map_file_name = path( g_tmp_dir ) / path( g_valid_map_name );
saveParameters.image_format = "";
saveParameters.free_thresh = 2.0;
saveParameters.occupied_thresh = 2.0;
saveParameters.mode = MapMode::Trinary;
ASSERT_FALSE( saveMapToFile( map_msg, saveParameters ) );
saveParameters.free_thresh = -2.0;
saveParameters.occupied_thresh = -2.0;
ASSERT_FALSE( saveMapToFile( map_msg, saveParameters ) );
saveParameters.free_thresh = 0.7;
saveParameters.occupied_thresh = 0.2;
ASSERT_FALSE( saveMapToFile( map_msg, saveParameters ) );
saveParameters.free_thresh = 0.0;
saveParameters.occupied_thresh = 0.0;
ASSERT_TRUE( saveMapToFile( map_msg, saveParameters ) );
saveParameters.map_file_name = path( "/invalid_path" ) / path( g_valid_map_name );
ASSERT_FALSE( saveMapToFile( map_msg, saveParameters ) );
}
// Load valid YAML file and check for consistency
292 TEST_F( MapIOTester, loadValidYAML )
{
LoadParameters loadParameters;
ASSERT_NO_THROW( loadParameters = loadMapYaml( path( TEST_DIR ) / path( g_valid_yaml_file ) ) );
LoadParameters refLoadParameters;
fillLoadParameters( path( TEST_DIR ) / path( g_valid_png_file ), refLoadParameters );
ASSERT_EQ( loadParameters.image_file_name, refLoadParameters.image_file_name );
ASSERT_FLOAT_EQ( loadParameters.resolution, refLoadParameters.resolution );
ASSERT_EQ( loadParameters.origin, refLoadParameters.origin );
ASSERT_FLOAT_EQ( loadParameters.free_thresh, refLoadParameters.free_thresh );
ASSERT_FLOAT_EQ( loadParameters.occupied_thresh, refLoadParameters.occupied_thresh );
ASSERT_EQ( loadParameters.mode, refLoadParameters.mode );
ASSERT_EQ( loadParameters.negate, refLoadParameters.negate );
}
// Try to load invalid YAML file
309 TEST_F( MapIOTester, loadInvalidYAML )
{
LoadParameters loadParameters;
ASSERT_ANY_THROW( loadParameters = loadMapYaml( path( TEST_DIR ) / path( "invalid_file.yaml" ) ) );
}
1 // Copyright ( c ) 2008, Willow Garage, Inc.
// All rights reserved.
//
// Software License Agreement ( BSD License 2.0 )
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above
// copyright notice, this list of conditions and the following
// disclaimer in the documentation and/or other materials provided
// with the distribution.
// * Neither the name of the Willow Garage nor the names of its
// contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
// LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Navigation function computation
// Uses Dijkstra's method
// Modified for Euclidean-distance computation
//
#ifndef NAV2_NAVFN_PLANNER__NAVFN_HPP_
#define NAV2_NAVFN_PLANNER__NAVFN_HPP_
#include <math.h>
#include <stdint.h>
#include <string.h>
#include <stdio.h>
namespace nav2_navfn_planner
{
// cost defs
#define COST_UNKNOWN_ROS 255 // 255 is unknown cost
#define COST_OBS 254 // 254 for forbidden regions
#define COST_OBS_ROS 253 // ROS values of 253 are obstacles
// navfn cost values are set to
// COST_NEUTRAL + COST_FACTOR * costmap_cost_value.
// Incoming costmap cost values are in the range 0 to 252.
// With COST_NEUTRAL of 50, the COST_FACTOR needs to be about 0.8 to
// ensure the input values are spread evenly over the output range, 50
// to 253. If COST_FACTOR is higher, cost values will have a plateau
// around obstacles and the planner will then treat ( for example ) the
// whole width of a narrow hallway as equally undesirable and thus
// will not plan paths down the center.
#define COST_NEUTRAL 50 // Set this to "open space" value
#define COST_FACTOR 0.8 // Used for translating costs in NavFn::setCostmap( )
// Define the cost type in the case that it is not set. However, this allows
// clients to modify it without changing the file. Arguably, it is better to require it to
// be defined by a user explicitly
#ifndef COSTTYPE
#define COSTTYPE unsigned char // Whatever is used...
#endif
// potential defs
#define POT_HIGH 1.0e10 // unassigned cell potential
// priority buffers
#define PRIORITYBUFSIZE 10000
/**
Navigation function call.
\param costmap Cost map array, of type COSTTYPE; origin is upper left
NOTE: will be modified to have a border of obstacle costs
\param nx Width of map in cells
\param ny Height of map in cells
\param goal X, Y position of goal cell
\param start X, Y position of start cell
Returns length of plan if found, and fills an array with x, y interpolated
positions at about 1/2 cell resolution; else returns 0.
*/
93 int create_nav_plan_astar(
const COSTTYPE * costmap, int nx, int ny,
int * goal, int * start,
float * plan, int nplan );
/**
* @class NavFn
* @brief Navigation function class. Holds buffers for costmap, navfn map. Maps are pixel-based.
* Origin is upper left, x is right, y is down.
*/
103 class NavFn
{
public:
/**
* @brief Constructs the planner
* @param nx The x size of the map
* @param ny The y size of the map
*/
111 NavFn( int nx, int ny );
113 ~NavFn( );
/**
* @brief Sets or resets the size of the map
* @param nx The x size of the map
* @param ny The y size of the map
*/
120 void setNavArr( int nx, int ny );
int nx, ny, ns; /**< size of grid, in pixels */
/**
* @brief Set up the cost array for the planner, usually from ROS
* @param cmap The costmap
* @param isROS Whether or not the costmap is coming in in ROS format
* @param allow_unknown Whether or not the planner should be allowed to plan through
* unknown space
*/
130 void setCostmap( const COSTTYPE * cmap, bool isROS = true, bool allow_unknown = true );
/**
* @brief Calculates a plan using the A* heuristic, returns true if one is found
* @return True if a plan is found, false otherwise
*/
136 bool calcNavFnAstar( );
/**
* @brief Caclulates the full navigation function using Dijkstra
*/
141 bool calcNavFnDijkstra( bool atStart = false );
/**
* @brief Accessor for the x-coordinates of a path
* @return The x-coordinates of a path
*/
147 float * getPathX( );
/**
* @brief Accessor for the y-coordinates of a path
* @return The y-coordinates of a path
*/
153 float * getPathY( );
/**
* @brief Accessor for the length of a path
* @return The length of a path, 0 if not found
*/
159 int getPathLen( );
/**
* @brief Gets the cost of the path found the last time a navigation function was computed
* @return The cost of the last path found
*/
165 float getLastPathCost( );
/** cell arrays */
168 COSTTYPE * costarr; /**< cost array in 2D configuration space */
float * potarr; /**< potential array, navigation function potential */
170 bool * pending; /**< pending cells during propagation */
int nobs; /**< number of obstacle cells */
/** block priority buffers */
int * pb1, * pb2, * pb3; /**< storage buffers for priority blocks */
int * curP, * nextP, * overP; /**< priority buffer block ptrs */
int curPe, nextPe, overPe; /**< end points of arrays */
/** block priority thresholds */
float curT; /**< current threshold */
float priInc; /**< priority threshold increment */
/** goal and start positions */
/**
* @brief Sets the goal position for the planner.
* Note: the navigation cost field computed gives the cost to get to a given point
* from the goal, not from the start.
* @param goal the goal position
*/
189 void setGoal( int * goal );
/**
* @brief Sets the start position for the planner.
* Note: the navigation cost field computed gives the cost to get to a given point
* from the goal, not from the start.
* @param start the start position
*/
197 void setStart( int * start );
int goal[2];
int start[2];
/**
* @brief Initialize cell k with cost v for propagation
* @param k the cell to initialize
* @param v the cost to give to the cell
*/
206 void initCost( int k, float v );
/** propagation */
/**
* @brief Updates the cell at index n
* @param n The index to update
*/
214 void updateCell( int n );
/**
* @brief Updates the cell at index n using the A* heuristic
* @param n The index to update
*/
220 void updateCellAstar( int n );
/**
* @brief Set up navigation potential arrays for new propagation
* @param keepit whether or not use COST_NEUTRAL
*/
226 void setupNavFn( bool keepit = false );
/**
* @brief Run propagation for <cycles> iterations, or until start is reached using
* breadth-first Dijkstra method
* @param cycles The maximum number of iterations to run for
* @param atStart Whether or not to stop when the start point is reached
* @return true if the start point is reached
*/
235 bool propNavFnDijkstra( int cycles, bool atStart = false );
/**
* @brief Run propagation for <cycles> iterations, or until start is reached using
* the best-first A* method with Euclidean distance heuristic
* @param cycles The maximum number of iterations to run for
* @return true if the start point is reached
*/
243 bool propNavFnAstar( int cycles ); /**< returns true if start point found */
/** gradient and paths */
float * gradx, * grady; /**< gradient arrays, size of potential array */
float * pathx, * pathy; /**< path points, as subpixel cell coordinates */
int npath; /**< number of path points */
int npathbuf; /**< size of pathx, pathy buffers */
float last_path_cost_; /**< Holds the cost of the path found the last time A* was called */
/**
* @brief Calculates the path for at mose <n> cycles
* @param n The maximum number of cycles to run for
* @return The lenght of the path found, 0 if none
*/
258 int calcPath( int n, int * st = NULL );
/**
* @brief Calculate gradient at a cell
* @param n Cell number <n>
* @return float norm
*/
265 float gradCell( int n ); /**< calculates gradient at cell <n>, returns norm */
float pathStep; /**< step size for following gradient */
/** display callback */
/**< <n> is the number of cycles between updates */
// void display( void fn( NavFn * nav ), int n = 100 );
// int displayInt; /**< save second argument of display( ) above */
// void ( * displayFn )( NavFn * nav ); /**< display function itself */
/** save costmap */
/**< write out costmap and start/goal states as fname.pgm and fname.txt */
// void savemap( const char * fname );
};
} // namespace nav2_navfn_planner
#endif // NAV2_NAVFN_PLANNER__NAVFN_HPP_
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2018 Simbe Robotics
// Copyright ( c ) 2019 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_NAVFN_PLANNER__NAVFN_PLANNER_HPP_
#define NAV2_NAVFN_PLANNER__NAVFN_PLANNER_HPP_
#include <chrono>
#include <string>
#include <memory>
#include <vector>
#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_core/global_planner.hpp"
#include "nav_msgs/msg/path.hpp"
#include "nav2_navfn_planner/navfn.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav2_util/geometry_utils.hpp"
namespace nav2_navfn_planner
{
38 class NavfnPlanner : public nav2_core::GlobalPlanner
{
public:
/**
* @brief constructor
*/
44 NavfnPlanner( );
/**
* @brief destructor
*/
49 ~NavfnPlanner( );
/**
* @brief Configuring plugin
* @param parent Lifecycle node pointer
* @param name Name of plugin map
* @param tf Shared ptr of TF2 buffer
* @param costmap_ros Costmap2DROS object
*/
void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros ) override;
/**
* @brief Cleanup lifecycle node
*/
void cleanup( ) override;
/**
* @brief Activate lifecycle node
*/
void activate( ) override;
/**
* @brief Deactivate lifecycle node
*/
void deactivate( ) override;
/**
* @brief Creating a plan from start and goal poses
* @param start Start pose
* @param goal Goal pose
* @return nav_msgs::Path of the generated path
*/
nav_msgs::msg::Path createPlan(
const geometry_msgs::msg::PoseStamped & start,
const geometry_msgs::msg::PoseStamped & goal ) override;
protected:
/**
* @brief Compute a plan given start and goal poses, provided in global world frame.
* @param start Start pose
* @param goal Goal pose
* @param tolerance Relaxation constraint in x and y
* @param plan Path to be computed
* @return true if can find the path
*/
bool makePlan(
const geometry_msgs::msg::Pose & start,
const geometry_msgs::msg::Pose & goal, double tolerance,
nav_msgs::msg::Path & plan );
/**
* @brief Compute the navigation function given a seed point in the world to start from
* @param world_point Point in world coordinate frame
* @return true if can compute
*/
bool computePotential( const geometry_msgs::msg::Point & world_point );
/**
* @brief Compute a plan to a goal from a potential - must call computePotential first
* @param goal Goal pose
* @param plan Path to be computed
* @return true if can compute a plan path
*/
bool getPlanFromPotential(
const geometry_msgs::msg::Pose & goal,
nav_msgs::msg::Path & plan );
/**
* @brief Remove artifacts at the end of the path - originated from planning on a discretized world
* @param goal Goal pose
* @param plan Computed path
*/
void smoothApproachToGoal(
const geometry_msgs::msg::Pose & goal,
nav_msgs::msg::Path & plan );
/**
* @brief Compute the potential, or navigation cost, at a given point in the world
* must call computePotential first
* @param world_point Point in world coordinate frame
* @return double point potential ( navigation cost )
*/
double getPointPotential( const geometry_msgs::msg::Point & world_point );
// Check for a valid potential value at a given point in the world
// - must call computePotential first
// - currently unused
// bool validPointPotential( const geometry_msgs::msg::Point & world_point );
// bool validPointPotential( const geometry_msgs::msg::Point & world_point, double tolerance );
/**
* @brief Compute the squared distance between two points
* @param p1 Point 1
* @param p2 Point 2
* @return double squared distance between two points
*/
inline double squared_distance(
const geometry_msgs::msg::Pose & p1,
const geometry_msgs::msg::Pose & p2 )
{
double dx = p1.position.x - p2.position.x;
double dy = p1.position.y - p2.position.y;
return dx * dx + dy * dy;
}
/**
* @brief Transform a point from world to map frame
* @param wx double of world X coordinate
* @param wy double of world Y coordinate
* @param mx int of map X coordinate
* @param my int of map Y coordinate
* @return true if can transform
*/
bool worldToMap( double wx, double wy, unsigned int & mx, unsigned int & my );
/**
* @brief Transform a point from map to world frame
* @param mx double of map X coordinate
* @param my double of map Y coordinate
* @param wx double of world X coordinate
* @param wy double of world Y coordinate
*/
175 void mapToWorld( double mx, double my, double & wx, double & wy );
/**
* @brief Set the corresponding cell cost to be free space
* @param mx int of map X coordinate
* @param my int of map Y coordinate
*/
182 void clearRobotCell( unsigned int mx, unsigned int my );
/**
* @brief Determine if a new planner object should be made
* @return true if planner object is out of date
*/
188 bool isPlannerOutOfDate( );
// Planner based on ROS1 NavFn algorithm
std::unique_ptr<NavFn> planner_;
// TF buffer
std::shared_ptr<tf2_ros::Buffer> tf_;
// Clock
rclcpp::Clock::SharedPtr clock_;
// Logger
200 rclcpp::Logger logger_{rclcpp::get_logger( "NavfnPlanner" )};
// Global Costmap
nav2_costmap_2d::Costmap2D * costmap_;
// The global frame of the costmap
std::string global_frame_, name_;
// Whether or not the planner should be allowed to plan through unknown space
bool allow_unknown_, use_final_approach_orientation_;
// If the goal is obstructed, the tolerance specifies how many meters the planner
// can relax the constraint in x and y before failing
double tolerance_;
// Whether to use the astar planner or default dijkstras
bool use_astar_;
// parent node weak ptr
rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
// Dynamic parameters handler
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
/**
* @brief Callback executed when a paramter change is detected
* @param parameters list of changed parameters
*/
rcl_interfaces::msg::SetParametersResult
229 dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters );
};
} // namespace nav2_navfn_planner
#endif // NAV2_NAVFN_PLANNER__NAVFN_PLANNER_HPP_
1 // Copyright ( c ) 2008, Willow Garage, Inc.
// All rights reserved.
//
// Software License Agreement ( BSD License 2.0 )
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above
// copyright notice, this list of conditions and the following
// disclaimer in the documentation and/or other materials provided
// with the distribution.
// * Neither the name of the Willow Garage nor the names of its
// contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
// LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Navigation function computation
// Uses Dijkstra's method
// Modified for Euclidean-distance computation
//
// Path calculation uses no interpolation when pot field is at max in
// nearby cells
//
// Path calc has sanity check that it succeeded
//
#include "nav2_navfn_planner/navfn.hpp"
#include <algorithm>
#include "rclcpp/rclcpp.hpp"
namespace nav2_navfn_planner
{
//
// function to perform nav fn calculation
// keeps track of internal buffers, will be more efficient
// if the size of the environment does not change
//
// Example usage:
/*
int
create_nav_plan_astar(
COSTTYPE * costmap, int nx, int ny,
int * goal, int * start,
float * plan, int nplan )
{
static NavFn * nav = NULL;
if ( nav == NULL ) {
nav = new NavFn( nx, ny );
}
if ( nav->nx != nx || nav->ny != ny ) { // check for compatibility with previous call
delete nav;
nav = new NavFn( nx, ny );
}
nav->setGoal( goal );
nav->setStart( start );
nav->costarr = costmap;
nav->setupNavFn( true );
// calculate the nav fn and path
nav->priInc = 2 * COST_NEUTRAL;
nav->propNavFnAstar( std::max( nx * ny / 20, nx + ny ) );
// path
int len = nav->calcPath( nplan );
if ( len > 0 ) { // found plan
RCLCPP_DEBUG( rclcpp::get_logger( "rclcpp" ), "[NavFn] Path found, %d steps\n", len );
} else {
RCLCPP_DEBUG( rclcpp::get_logger( "rclcpp" ), "[NavFn] No path found\n" );
}
if ( len > 0 ) {
for ( int i = 0; i < len; i++ ) {
plan[i * 2] = nav->pathx[i];
plan[i * 2 + 1] = nav->pathy[i];
}
}
return len;
}
*/
//
// create nav fn buffers
//
111 NavFn::NavFn( int xs, int ys )
{
// create cell arrays
costarr = NULL;
potarr = NULL;
pending = NULL;
gradx = grady = NULL;
setNavArr( xs, ys );
// priority buffers
pb1 = new int[PRIORITYBUFSIZE];
pb2 = new int[PRIORITYBUFSIZE];
pb3 = new int[PRIORITYBUFSIZE];
// for Dijkstra ( breadth-first ), set to COST_NEUTRAL
// for A* ( best-first ), set to COST_NEUTRAL
priInc = 2 * COST_NEUTRAL;
// goal and start
goal[0] = goal[1] = 0;
start[0] = start[1] = 0;
// display function
// displayFn = NULL;
// displayInt = 0;
// path buffers
npathbuf = npath = 0;
pathx = pathy = NULL;
pathStep = 0.5;
}
144 NavFn::~NavFn( )
{
if ( costarr ) {
delete[] costarr;
}
if ( potarr ) {
delete[] potarr;
}
if ( pending ) {
delete[] pending;
}
if ( gradx ) {
delete[] gradx;
}
if ( grady ) {
delete[] grady;
}
if ( pathx ) {
delete[] pathx;
}
if ( pathy ) {
delete[] pathy;
}
if ( pb1 ) {
delete[] pb1;
}
if ( pb2 ) {
delete[] pb2;
}
if ( pb3 ) {
delete[] pb3;
}
}
//
// set goal, start positions for the nav fn
//
void
184 NavFn::setGoal( int * g )
{
goal[0] = g[0];
goal[1] = g[1];
RCLCPP_DEBUG( rclcpp::get_logger( "rclcpp" ), "[NavFn] Setting goal to %d, %d\n", goal[0], goal[1] );
}
void
192 NavFn::setStart( int * g )
{
start[0] = g[0];
start[1] = g[1];
RCLCPP_DEBUG(
rclcpp::get_logger( "rclcpp" ), "[NavFn] Setting start to %d, %d\n", start[0],
start[1] );
}
//
// Set/Reset map size
//
void
206 NavFn::setNavArr( int xs, int ys )
{
RCLCPP_DEBUG( rclcpp::get_logger( "rclcpp" ), "[NavFn] Array is %d x %d\n", xs, ys );
nx = xs;
ny = ys;
ns = nx * ny;
if ( costarr ) {
delete[] costarr;
}
if ( potarr ) {
delete[] potarr;
}
if ( pending ) {
delete[] pending;
}
if ( gradx ) {
delete[] gradx;
}
if ( grady ) {
delete[] grady;
}
costarr = new COSTTYPE[ns]; // cost array, 2d config space
memset( costarr, 0, ns * sizeof( COSTTYPE ) );
potarr = new float[ns]; // navigation potential array
pending = new bool[ns];
memset( pending, 0, ns * sizeof( bool ) );
gradx = new float[ns];
grady = new float[ns];
}
//
// set up cost array, usually from ROS
//
void
246 NavFn::setCostmap( const COSTTYPE * cmap, bool isROS, bool allow_unknown )
{
COSTTYPE * cm = costarr;
if ( isROS ) { // ROS-type cost array
for ( int i = 0; i < ny; i++ ) {
int k = i * nx;
for ( int j = 0; j < nx; j++, k++, cmap++, cm++ ) {
// This transforms the incoming cost values:
// COST_OBS -> COST_OBS ( incoming "lethal obstacle" )
// COST_OBS_ROS -> COST_OBS ( incoming "inscribed inflated obstacle" )
// values in range 0 to 252 -> values from COST_NEUTRAL to COST_OBS_ROS.
*cm = COST_OBS;
int v = *cmap;
if ( v < COST_OBS_ROS ) {
v = COST_NEUTRAL + COST_FACTOR * v;
if ( v >= COST_OBS ) {
v = COST_OBS - 1;
}
*cm = v;
} else if ( v == COST_UNKNOWN_ROS && allow_unknown ) {
v = COST_OBS - 1;
*cm = v;
}
}
}
} else { // not a ROS map, just a PGM
for ( int i = 0; i < ny; i++ ) {
int k = i * nx;
for ( int j = 0; j < nx; j++, k++, cmap++, cm++ ) {
*cm = COST_OBS;
if ( i < 7 || i > ny - 8 || j < 7 || j > nx - 8 ) {
continue; // don't do borders
}
int v = *cmap;
if ( v < COST_OBS_ROS ) {
v = COST_NEUTRAL + COST_FACTOR * v;
if ( v >= COST_OBS ) {
v = COST_OBS - 1;
}
*cm = v;
} else if ( v == COST_UNKNOWN_ROS ) {
v = COST_OBS - 1;
*cm = v;
}
}
}
}
}
bool
296 NavFn::calcNavFnDijkstra( bool atStart )
{
setupNavFn( true );
// calculate the nav fn and path
return propNavFnDijkstra( std::max( nx * ny / 20, nx + ny ), atStart );
}
//
// calculate navigation function, given a costmap, goal, and start
//
bool
310 NavFn::calcNavFnAstar( )
{
setupNavFn( true );
// calculate the nav fn and path
return propNavFnAstar( std::max( nx * ny / 20, nx + ny ) );
}
//
// returning values
//
322 float * NavFn::getPathX( ) {return pathx;}
323 float * NavFn::getPathY( ) {return pathy;}
324 int NavFn::getPathLen( ) {return npath;}
// inserting onto the priority blocks
#define push_cur( n ) {if ( n >= 0 && n < ns && !pending[n] && \
costarr[n] < COST_OBS && curPe < PRIORITYBUFSIZE ) \
{curP[curPe++] = n; pending[n] = true;}}
#define push_next( n ) {if ( n >= 0 && n < ns && !pending[n] && \
costarr[n] < COST_OBS && nextPe < PRIORITYBUFSIZE ) \
{nextP[nextPe++] = n; pending[n] = true;}}
#define push_over( n ) {if ( n >= 0 && n < ns && !pending[n] && \
costarr[n] < COST_OBS && overPe < PRIORITYBUFSIZE ) \
335 {overP[overPe++] = n; pending[n] = true;}}
// Set up navigation potential arrays for new propagation
void
NavFn::setupNavFn( bool keepit )
{
// reset values in propagation arrays
for ( int i = 0; i < ns; i++ ) {
potarr[i] = POT_HIGH;
if ( !keepit ) {
costarr[i] = COST_NEUTRAL;
}
gradx[i] = grady[i] = 0.0;
}
// outer bounds of cost array
COSTTYPE * pc;
pc = costarr;
for ( int i = 0; i < nx; i++ ) {
*pc++ = COST_OBS;
}
pc = costarr + ( ny - 1 ) * nx;
for ( int i = 0; i < nx; i++ ) {
*pc++ = COST_OBS;
}
pc = costarr;
for ( int i = 0; i < ny; i++, pc += nx ) {
*pc = COST_OBS;
}
pc = costarr + nx - 1;
for ( int i = 0; i < ny; i++, pc += nx ) {
*pc = COST_OBS;
}
// priority buffers
curT = COST_OBS;
curP = pb1;
curPe = 0;
nextP = pb2;
nextPe = 0;
overP = pb3;
overPe = 0;
memset( pending, 0, ns * sizeof( bool ) );
// set goal
int k = goal[0] + goal[1] * nx;
initCost( k, 0 );
// find # of obstacle cells
pc = costarr;
int ntot = 0;
for ( int i = 0; i < ns; i++, pc++ ) {
if ( *pc >= COST_OBS ) {
ntot++; // number of cells that are obstacles
}
}
nobs = ntot;
394 }
// initialize a goal-type cost for starting propagation
void
NavFn::initCost( int k, float v )
{
potarr[k] = v;
push_cur( k + 1 );
push_cur( k - 1 );
push_cur( k - nx );
push_cur( k + nx );
}
//
// Critical function: calculate updated potential value of a cell,
// given its neighbors' values
// Planar-wave update calculation from two lowest neighbors in a 4-grid
// Quadratic approximation to the interpolated value
415 // No checking of bounds here, this function should be fast
//
#define INVSQRT2 0.707106781
inline void
NavFn::updateCell( int n )
{
// get neighbors
float u, d, l, r;
l = potarr[n - 1];
r = potarr[n + 1];
u = potarr[n - nx];
d = potarr[n + nx];
// ROS_INFO( "[Update] c: %0.1f l: %0.1f r: %0.1f u: %0.1f d: %0.1f\n",
// potarr[n], l, r, u, d );
// ROS_INFO( "[Update] cost: %d\n", costarr[n] );
// find lowest, and its lowest neighbor
float ta, tc;
if ( l < r ) {tc = l;} else {tc = r;}
if ( u < d ) {ta = u;} else {ta = d;}
// do planar wave update
if ( costarr[n] < COST_OBS ) { // don't propagate into obstacles
float hf = static_cast<float>( costarr[n] ); // traversability factor
float dc = tc - ta; // relative cost between ta, tc
if ( dc < 0 ) { // ta is lowest
dc = -dc;
ta = tc;
}
// calculate new potential
float pot;
if ( dc >= hf ) { // if too large, use ta-only update
pot = ta + hf;
} else { // two-neighbor interpolation update
// use quadratic approximation
// might speed this up through table lookup, but still have to
// do the divide
float d = dc / hf;
float v = -0.2301 * d * d + 0.5307 * d + 0.7040;
pot = ta + hf * v;
}
// ROS_INFO( "[Update] new pot: %d\n", costarr[n] );
// now add affected neighbors to priority blocks
if ( pot < potarr[n] ) {
float le = INVSQRT2 * static_cast<float>( costarr[n - 1] );
float re = INVSQRT2 * static_cast<float>( costarr[n + 1] );
float ue = INVSQRT2 * static_cast<float>( costarr[n - nx] );
float de = INVSQRT2 * static_cast<float>( costarr[n + nx] );
potarr[n] = pot;
if ( pot < curT ) { // low-cost buffer block
if ( l > pot + le ) {push_next( n - 1 );}
if ( r > pot + re ) {push_next( n + 1 );}
if ( u > pot + ue ) {push_next( n - nx );}
if ( d > pot + de ) {push_next( n + nx );}
} else { // overflow block
if ( l > pot + le ) {push_over( n - 1 );}
if ( r > pot + re ) {push_over( n + 1 );}
if ( u > pot + ue ) {push_over( n - nx );}
if ( d > pot + de ) {push_over( n + nx );}
}
}
}
}
//
// Use A* method for setting priorities
// Critical function: calculate updated potential value of a cell,
// given its neighbors' values
// Planar-wave update calculation from two lowest neighbors in a 4-grid
// Quadratic approximation to the interpolated value
490 // No checking of bounds here, this function should be fast
//
#define INVSQRT2 0.707106781
inline void
NavFn::updateCellAstar( int n )
{
// get neighbors
float u, d, l, r;
l = potarr[n - 1];
r = potarr[n + 1];
u = potarr[n - nx];
d = potarr[n + nx];
// ROS_INFO( "[Update] c: %0.1f l: %0.1f r: %0.1f u: %0.1f d: %0.1f\n",
// potarr[n], l, r, u, d );
// ROS_INFO( "[Update] cost of %d: %d\n", n, costarr[n] );
// find lowest, and its lowest neighbor
float ta, tc;
if ( l < r ) {tc = l;} else {tc = r;}
if ( u < d ) {ta = u;} else {ta = d;}
// do planar wave update
if ( costarr[n] < COST_OBS ) { // don't propagate into obstacles
float hf = static_cast<float>( costarr[n] ); // traversability factor
float dc = tc - ta; // relative cost between ta, tc
if ( dc < 0 ) { // ta is lowest
dc = -dc;
ta = tc;
}
// calculate new potential
float pot;
if ( dc >= hf ) { // if too large, use ta-only update
pot = ta + hf;
} else { // two-neighbor interpolation update
// use quadratic approximation
// might speed this up through table lookup, but still have to
// do the divide
float d = dc / hf;
float v = -0.2301 * d * d + 0.5307 * d + 0.7040;
pot = ta + hf * v;
}
// ROS_INFO( "[Update] new pot: %d\n", costarr[n] );
// now add affected neighbors to priority blocks
if ( pot < potarr[n] ) {
float le = INVSQRT2 * static_cast<float>( costarr[n - 1] );
float re = INVSQRT2 * static_cast<float>( costarr[n + 1] );
float ue = INVSQRT2 * static_cast<float>( costarr[n - nx] );
float de = INVSQRT2 * static_cast<float>( costarr[n + nx] );
// calculate distance
int x = n % nx;
int y = n / nx;
float dist = hypot( x - start[0], y - start[1] ) * static_cast<float>( COST_NEUTRAL );
potarr[n] = pot;
pot += dist;
if ( pot < curT ) { // low-cost buffer block
if ( l > pot + le ) {push_next( n - 1 );}
if ( r > pot + re ) {push_next( n + 1 );}
if ( u > pot + ue ) {push_next( n - nx );}
if ( d > pot + de ) {push_next( n + nx );}
} else {
if ( l > pot + le ) {push_over( n - 1 );}
if ( r > pot + re ) {push_over( n + 1 );}
if ( u > pot + ue ) {push_over( n - nx );}
if ( d > pot + de ) {push_over( n + nx );}
}
}
}
}
//
// main propagation function
// Dijkstra method, breadth-first
570 // runs for a specified number of cycles,
// or until it runs out of cells to update,
// or until the Start cell is found ( atStart = true )
//
bool
NavFn::propNavFnDijkstra( int cycles, bool atStart )
{
int nwv = 0; // max priority block size
int nc = 0; // number of cells put into priority blocks
int cycle = 0; // which cycle we're on
// set up start cell
int startCell = start[1] * nx + start[0];
for ( ; cycle < cycles; cycle++ ) { // go for this many cycles, unless interrupted
if ( curPe == 0 && nextPe == 0 ) { // priority blocks empty
break;
}
// stats
nc += curPe;
if ( curPe > nwv ) {
nwv = curPe;
}
// reset pending flags on current priority buffer
int * pb = curP;
int i = curPe;
while ( i-- > 0 ) {
pending[*( pb++ )] = false;
}
// process current priority buffer
pb = curP;
i = curPe;
while ( i-- > 0 ) {
updateCell( *pb++ );
}
// if ( displayInt > 0 && ( cycle % displayInt ) == 0 ) {
// displayFn( this );
// }
// swap priority blocks curP <=> nextP
curPe = nextPe;
nextPe = 0;
pb = curP; // swap buffers
curP = nextP;
nextP = pb;
// see if we're done with this priority level
if ( curPe == 0 ) {
curT += priInc; // increment priority threshold
curPe = overPe; // set current to overflow block
overPe = 0;
pb = curP; // swap buffers
curP = overP;
overP = pb;
}
// check if we've hit the Start cell
if ( atStart ) {
if ( potarr[startCell] < POT_HIGH ) {
break;
}
}
}
RCLCPP_DEBUG(
rclcpp::get_logger( "rclcpp" ),
"[NavFn] Used %d cycles, %d cells visited ( %d%% ), priority buf max %d\n",
cycle, nc, ( int )( ( nc * 100.0 ) / ( ns - nobs ) ), nwv );
return ( cycle < cycles ) ? true : false;
}
//
// main propagation function
// A* method, best-first
// uses Euclidean distance heuristic
651 // runs for a specified number of cycles,
// or until it runs out of cells to update,
// or until the Start cell is found ( atStart = true )
//
bool
NavFn::propNavFnAstar( int cycles )
{
int nwv = 0; // max priority block size
int nc = 0; // number of cells put into priority blocks
int cycle = 0; // which cycle we're on
// set initial threshold, based on distance
float dist = hypot( goal[0] - start[0], goal[1] - start[1] ) * static_cast<float>( COST_NEUTRAL );
curT = dist + curT;
// set up start cell
int startCell = start[1] * nx + start[0];
// do main cycle
for ( ; cycle < cycles; cycle++ ) { // go for this many cycles, unless interrupted
if ( curPe == 0 && nextPe == 0 ) { // priority blocks empty
break;
}
// stats
nc += curPe;
if ( curPe > nwv ) {
nwv = curPe;
}
// reset pending flags on current priority buffer
int * pb = curP;
int i = curPe;
while ( i-- > 0 ) {
pending[*( pb++ )] = false;
}
// process current priority buffer
pb = curP;
i = curPe;
while ( i-- > 0 ) {
updateCellAstar( *pb++ );
}
// if ( displayInt > 0 && ( cycle % displayInt ) == 0 ) {
// displayFn( this );
// }
// swap priority blocks curP <=> nextP
curPe = nextPe;
nextPe = 0;
pb = curP; // swap buffers
curP = nextP;
nextP = pb;
// see if we're done with this priority level
if ( curPe == 0 ) {
curT += priInc; // increment priority threshold
curPe = overPe; // set current to overflow block
overPe = 0;
pb = curP; // swap buffers
curP = overP;
overP = pb;
}
// check if we've hit the Start cell
if ( potarr[startCell] < POT_HIGH ) {
break;
}
}
last_path_cost_ = potarr[startCell];
RCLCPP_DEBUG(
rclcpp::get_logger( "rclcpp" ),
"[NavFn] Used %d cycles, %d cells visited ( %d%% ), priority buf max %d\n",
cycle, nc, ( int )( ( nc * 100.0 ) / ( ns - nobs ) ), nwv );
if ( potarr[startCell] < POT_HIGH ) {
return true; // finished up here}
732 } else {
return false;
}
}
float NavFn::getLastPathCost( )
{
return last_path_cost_;
}
//
// Path construction
// Find gradient at array points, interpolate path
// Use step size of pathStep, usually 0.5 pixel
//
// Some sanity checks:
750 // 1. Stuck at same index position
// 2. Doesn't get near goal
// 3. Surrounded by high potentials
//
int
NavFn::calcPath( int n, int * st )
{
// test write
// savemap( "test" );
// check path arrays
if ( npathbuf < n ) {
if ( pathx ) {delete[] pathx;}
if ( pathy ) {delete[] pathy;}
pathx = new float[n];
pathy = new float[n];
npathbuf = n;
}
// set up start position at cell
// st is always upper left corner for 4-point bilinear interpolation
if ( st == NULL ) {st = start;}
int stc = st[1] * nx + st[0];
// set up offset
float dx = 0;
float dy = 0;
npath = 0;
// go for <n> cycles at most
for ( int i = 0; i < n; i++ ) {
// check if near goal
int nearest_point = std::max(
0,
std::min(
nx * ny - 1, stc + static_cast<int>( round( dx ) ) +
static_cast<int>( nx * round( dy ) ) ) );
if ( potarr[nearest_point] < COST_NEUTRAL ) {
pathx[npath] = static_cast<float>( goal[0] );
pathy[npath] = static_cast<float>( goal[1] );
return ++npath; // done!
}
if ( stc < nx || stc > ns - nx ) { // would be out of bounds
RCLCPP_DEBUG( rclcpp::get_logger( "rclcpp" ), "[PathCalc] Out of bounds" );
return 0;
}
// add to path
pathx[npath] = stc % nx + dx;
pathy[npath] = stc / nx + dy;
npath++;
bool oscillation_detected = false;
if ( npath > 2 &&
pathx[npath - 1] == pathx[npath - 3] &&
pathy[npath - 1] == pathy[npath - 3] )
{
RCLCPP_DEBUG(
rclcpp::get_logger( "rclcpp" ),
"[PathCalc] oscillation detected, attempting fix." );
oscillation_detected = true;
}
int stcnx = stc + nx;
int stcpx = stc - nx;
// check for potentials at eight positions near cell
if ( potarr[stc] >= POT_HIGH ||
potarr[stc + 1] >= POT_HIGH ||
potarr[stc - 1] >= POT_HIGH ||
potarr[stcnx] >= POT_HIGH ||
potarr[stcnx + 1] >= POT_HIGH ||
potarr[stcnx - 1] >= POT_HIGH ||
potarr[stcpx] >= POT_HIGH ||
potarr[stcpx + 1] >= POT_HIGH ||
potarr[stcpx - 1] >= POT_HIGH ||
oscillation_detected )
{
RCLCPP_DEBUG(
rclcpp::get_logger( "rclcpp" ),
"[Path] Pot fn boundary, following grid ( %0.1f/%d )", potarr[stc], npath );
// check eight neighbors to find the lowest
int minc = stc;
int minp = potarr[stc];
int st = stcpx - 1;
if ( potarr[st] < minp ) {minp = potarr[st]; minc = st;}
st++;
if ( potarr[st] < minp ) {minp = potarr[st]; minc = st;}
st++;
if ( potarr[st] < minp ) {minp = potarr[st]; minc = st;}
st = stc - 1;
if ( potarr[st] < minp ) {minp = potarr[st]; minc = st;}
st = stc + 1;
if ( potarr[st] < minp ) {minp = potarr[st]; minc = st;}
st = stcnx - 1;
if ( potarr[st] < minp ) {minp = potarr[st]; minc = st;}
st++;
if ( potarr[st] < minp ) {minp = potarr[st]; minc = st;}
st++;
if ( potarr[st] < minp ) {minp = potarr[st]; minc = st;}
stc = minc;
dx = 0;
dy = 0;
RCLCPP_DEBUG(
rclcpp::get_logger( "rclcpp" ), "[Path] Pot: %0.1f pos: %0.1f, %0.1f",
potarr[stc], pathx[npath - 1], pathy[npath - 1] );
if ( potarr[stc] >= POT_HIGH ) {
RCLCPP_DEBUG( rclcpp::get_logger( "rclcpp" ), "[PathCalc] No path found, high potential" );
// savemap( "navfn_highpot" );
return 0;
}
} else { // have a good gradient here
// get grad at four positions near cell
gradCell( stc );
gradCell( stc + 1 );
gradCell( stcnx );
gradCell( stcnx + 1 );
// get interpolated gradient
float x1 = ( 1.0 - dx ) * gradx[stc] + dx * gradx[stc + 1];
float x2 = ( 1.0 - dx ) * gradx[stcnx] + dx * gradx[stcnx + 1];
float x = ( 1.0 - dy ) * x1 + dy * x2; // interpolated x
float y1 = ( 1.0 - dx ) * grady[stc] + dx * grady[stc + 1];
float y2 = ( 1.0 - dx ) * grady[stcnx] + dx * grady[stcnx + 1];
float y = ( 1.0 - dy ) * y1 + dy * y2; // interpolated y
#if 0
// show gradients
RCLCPP_DEBUG(
rclcpp::get_logger( "rclcpp" ),
"[Path] %0.2f, %0.2f %0.2f, %0.2f %0.2f, %0.2f %0.2f, %0.2f; final x=%.3f, y=%.3f\n",
gradx[stc], grady[stc], gradx[stc + 1], grady[stc + 1],
gradx[stcnx], grady[stcnx], gradx[stcnx + 1], grady[stcnx + 1],
x, y );
#endif
// check for zero gradient, failed
if ( x == 0.0 && y == 0.0 ) {
RCLCPP_DEBUG( rclcpp::get_logger( "rclcpp" ), "[PathCalc] Zero gradient" );
return 0;
}
// move in the right direction
float ss = pathStep / hypot( x, y );
dx += x * ss;
dy += y * ss;
// check for overflow
if ( dx > 1.0 ) {stc++; dx -= 1.0;}
if ( dx < -1.0 ) {stc--; dx += 1.0;}
if ( dy > 1.0 ) {stc += nx; dy -= 1.0;}
if ( dy < -1.0 ) {stc -= nx; dy += 1.0;}
}
// ROS_INFO( "[Path] Pot: %0.1f grad: %0.1f, %0.1f pos: %0.1f, %0.1f\n",
// potarr[stc], x, y, pathx[npath-1], pathy[npath-1] );
}
// return npath; // out of cycles, return failure
RCLCPP_DEBUG( rclcpp::get_logger( "rclcpp" ), "[PathCalc] No path found, path too long" );
// savemap( "navfn_pathlong" );
return 0; // out of cycles, return failure
}
//
922 // gradient calculations
//
// calculate gradient at a cell
// positive value are to the right and down
float
NavFn::gradCell( int n )
{
if ( gradx[n] + grady[n] > 0.0 ) { // check this cell
return 1.0;
}
if ( n < nx || n > ns - nx ) { // would be out of bounds
return 0.0;
}
float cv = potarr[n];
float dx = 0.0;
float dy = 0.0;
// check for in an obstacle
if ( cv >= POT_HIGH ) {
if ( potarr[n - 1] < POT_HIGH ) {
dx = -COST_OBS;
} else if ( potarr[n + 1] < POT_HIGH ) {
dx = COST_OBS;
}
if ( potarr[n - nx] < POT_HIGH ) {
dy = -COST_OBS;
} else if ( potarr[n + nx] < POT_HIGH ) {
dy = COST_OBS;
}
} else { // not in an obstacle
// dx calc, average to sides
if ( potarr[n - 1] < POT_HIGH ) {
dx += potarr[n - 1] - cv;
}
if ( potarr[n + 1] < POT_HIGH ) {
dx += cv - potarr[n + 1];
}
// dy calc, average to sides
if ( potarr[n - nx] < POT_HIGH ) {
dy += potarr[n - nx] - cv;
}
if ( potarr[n + nx] < POT_HIGH ) {
dy += cv - potarr[n + nx];
}
}
// normalize
float norm = hypot( dx, dy );
if ( norm > 0 ) {
norm = 1.0 / norm;
gradx[n] = norm * dx;
grady[n] = norm * dy;
}
return norm;
}
//
// display function setup
// <n> is the number of cycles to wait before displaying,
// use 0 to turn it off
// void
// NavFn::display( void fn( NavFn * nav ), int n )
// {
// displayFn = fn;
// displayInt = n;
// }
//
// debug writes
// saves costmap and start/goal
//
// void
// NavFn::savemap( const char * fname )
// {
// char fn[4096];
// RCLCPP_DEBUG( rclcpp::get_logger( "rclcpp" ), "[NavFn] Saving costmap and start/goal points" );
// // write start and goal points
// snprintf( fn, sizeof( fn ), "%s.txt", fname );
// FILE * fp = fopen( fn, "w" );
// if ( !fp ) {
// RCLCPP_WARN( rclcpp::get_logger( "rclcpp" ), "Can't open file %s", fn );
// return;
// }
// fprintf( fp, "Goal: %d %d\nStart: %d %d\n", goal[0], goal[1], start[0], start[1] );
// fclose( fp );
// // write cost array
// if ( !costarr ) {
// return;
// }
// snprintf( fn, sizeof( fn ), "%s.pgm", fname );
// fp = fopen( fn, "wb" );
// if ( !fp ) {
// RCLCPP_WARN( rclcpp::get_logger( "rclcpp" ), "Can't open file %s", fn );
// return;
// }
// fprintf( fp, "P5\n%d\n%d\n%d\n", nx, ny, 0xff );
// fwrite( costarr, 1, nx * ny, fp );
// fclose( fp );
// }
} // namespace nav2_navfn_planner
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2018 Simbe Robotics
// Copyright ( c ) 2019 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// Navigation Strategy based on:
// Brock, O. and Oussama K. ( 1999 ). High-Speed Navigation Using
// the Global Dynamic Window Approach. IEEE.
// https://cs.stanford.edu/group/manips/publications/pdfs/Brock_1999_ICRA.pdf
// #define BENCHMARK_TESTING
#include "nav2_navfn_planner/navfn_planner.hpp"
#include <chrono>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <limits>
#include <memory>
#include <string>
#include <vector>
#include "builtin_interfaces/msg/duration.hpp"
#include "nav2_navfn_planner/navfn.hpp"
#include "nav2_util/costmap.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_costmap_2d/cost_values.hpp"
using namespace std::chrono_literals;
using namespace std::chrono; // NOLINT
using nav2_util::declare_parameter_if_not_declared;
using rcl_interfaces::msg::ParameterType;
using std::placeholders::_1;
namespace nav2_navfn_planner
{
50 NavfnPlanner::NavfnPlanner( )
: tf_( nullptr ), costmap_( nullptr )
{
}
55 NavfnPlanner::~NavfnPlanner( )
{
RCLCPP_INFO(
logger_, "Destroying plugin %s of type NavfnPlanner",
name_.c_str( ) );
}
void
63 NavfnPlanner::configure(
64 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
65 std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
66 std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros )
{
tf_ = tf;
name_ = name;
costmap_ = costmap_ros->getCostmap( );
global_frame_ = costmap_ros->getGlobalFrameID( );
node_ = parent;
auto node = parent.lock( );
clock_ = node->get_clock( );
logger_ = node->get_logger( );
RCLCPP_INFO(
logger_, "Configuring plugin %s of type NavfnPlanner",
name_.c_str( ) );
// Initialize parameters
// Declare this plugin's parameters
declare_parameter_if_not_declared( node, name + ".tolerance", rclcpp::ParameterValue( 0.5 ) );
node->get_parameter( name + ".tolerance", tolerance_ );
declare_parameter_if_not_declared( node, name + ".use_astar", rclcpp::ParameterValue( false ) );
node->get_parameter( name + ".use_astar", use_astar_ );
declare_parameter_if_not_declared( node, name + ".allow_unknown", rclcpp::ParameterValue( true ) );
node->get_parameter( name + ".allow_unknown", allow_unknown_ );
declare_parameter_if_not_declared(
node, name + ".use_final_approach_orientation", rclcpp::ParameterValue( false ) );
node->get_parameter( name + ".use_final_approach_orientation", use_final_approach_orientation_ );
// Create a planner based on the new costmap size
planner_ = std::make_unique<NavFn>(
costmap_->getSizeInCellsX( ),
costmap_->getSizeInCellsY( ) );
}
void
101 NavfnPlanner::activate( )
{
RCLCPP_INFO(
logger_, "Activating plugin %s of type NavfnPlanner",
name_.c_str( ) );
// Add callback for dynamic parameters
auto node = node_.lock( );
dyn_params_handler_ = node->add_on_set_parameters_callback(
std::bind( &NavfnPlanner::dynamicParametersCallback, this, _1 ) );
}
void
113 NavfnPlanner::deactivate( )
{
RCLCPP_INFO(
logger_, "Deactivating plugin %s of type NavfnPlanner",
name_.c_str( ) );
dyn_params_handler_.reset( );
}
void
122 NavfnPlanner::cleanup( )
{
RCLCPP_INFO(
logger_, "Cleaning up plugin %s of type NavfnPlanner",
name_.c_str( ) );
planner_.reset( );
}
130 nav_msgs::msg::Path NavfnPlanner::createPlan(
131 const geometry_msgs::msg::PoseStamped & start,
132 const geometry_msgs::msg::PoseStamped & goal )
{
#ifdef BENCHMARK_TESTING
steady_clock::time_point a = steady_clock::now( );
#endif
// Update planner based on the new costmap size
if ( isPlannerOutOfDate( ) ) {
planner_->setNavArr(
costmap_->getSizeInCellsX( ),
costmap_->getSizeInCellsY( ) );
}
nav_msgs::msg::Path path;
// Corner case of the start( x, y ) = goal( x, y )
if ( start.pose.position.x == goal.pose.position.x &&
start.pose.position.y == goal.pose.position.y )
{
unsigned int mx, my;
costmap_->worldToMap( start.pose.position.x, start.pose.position.y, mx, my );
if ( costmap_->getCost( mx, my ) == nav2_costmap_2d::LETHAL_OBSTACLE ) {
RCLCPP_WARN( logger_, "Failed to create a unique pose path because of obstacles" );
return path;
}
path.header.stamp = clock_->now( );
path.header.frame_id = global_frame_;
geometry_msgs::msg::PoseStamped pose;
pose.header = path.header;
pose.pose.position.z = 0.0;
pose.pose = start.pose;
// if we have a different start and goal orientation, set the unique path pose to the goal
// orientation, unless use_final_approach_orientation=true where we need it to be the start
// orientation to avoid movement from the local planner
if ( start.pose.orientation != goal.pose.orientation && !use_final_approach_orientation_ ) {
pose.pose.orientation = goal.pose.orientation;
}
path.poses.push_back( pose );
return path;
}
if ( !makePlan( start.pose, goal.pose, tolerance_, path ) ) {
RCLCPP_WARN(
logger_, "%s: failed to create plan with "
"tolerance %.2f.", name_.c_str( ), tolerance_ );
}
#ifdef BENCHMARK_TESTING
steady_clock::time_point b = steady_clock::now( );
duration<double> time_span = duration_cast<duration<double>>( b - a );
std::cout << "It took " << time_span.count( ) * 1000 << std::endl;
#endif
return path;
}
bool
191 NavfnPlanner::isPlannerOutOfDate( )
{
if ( !planner_.get( ) ||
planner_->nx != static_cast<int>( costmap_->getSizeInCellsX( ) ) ||
planner_->ny != static_cast<int>( costmap_->getSizeInCellsY( ) ) )
{
return true;
}
return false;
}
bool
203 NavfnPlanner::makePlan(
204 const geometry_msgs::msg::Pose & start,
205 const geometry_msgs::msg::Pose & goal, double tolerance,
206 nav_msgs::msg::Path & plan )
{
// clear the plan, just in case
plan.poses.clear( );
plan.header.stamp = clock_->now( );
plan.header.frame_id = global_frame_;
// TODO( orduno ): add checks for start and goal reference frame -- should be in global frame
double wx = start.position.x;
double wy = start.position.y;
RCLCPP_DEBUG(
logger_, "Making plan from ( %.2f, %.2f ) to ( %.2f, %.2f )",
start.position.x, start.position.y, goal.position.x, goal.position.y );
unsigned int mx, my;
if ( !worldToMap( wx, wy, mx, my ) ) {
RCLCPP_WARN(
logger_,
"Cannot create a plan: the robot's start position is off the global"
" costmap. Planning will always fail, are you sure"
" the robot has been properly localized?" );
return false;
}
// clear the starting cell within the costmap because we know it can't be an obstacle
clearRobotCell( mx, my );
std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock( *( costmap_->getMutex( ) ) );
// make sure to resize the underlying array that Navfn uses
planner_->setNavArr(
costmap_->getSizeInCellsX( ),
costmap_->getSizeInCellsY( ) );
planner_->setCostmap( costmap_->getCharMap( ), true, allow_unknown_ );
lock.unlock( );
int map_start[2];
map_start[0] = mx;
map_start[1] = my;
wx = goal.position.x;
wy = goal.position.y;
if ( !worldToMap( wx, wy, mx, my ) ) {
RCLCPP_WARN(
logger_,
"The goal sent to the planner is off the global costmap."
" Planning will always fail to this goal." );
return false;
}
int map_goal[2];
map_goal[0] = mx;
map_goal[1] = my;
// TODO( orduno ): Explain why we are providing 'map_goal' to setStart( ).
// Same for setGoal, seems reversed. Computing backwards?
planner_->setStart( map_goal );
planner_->setGoal( map_start );
if ( use_astar_ ) {
planner_->calcNavFnAstar( );
} else {
planner_->calcNavFnDijkstra( true );
}
double resolution = costmap_->getResolution( );
geometry_msgs::msg::Pose p, best_pose;
bool found_legal = false;
p = goal;
double potential = getPointPotential( p.position );
if ( potential < POT_HIGH ) {
// Goal is reachable by itself
best_pose = p;
found_legal = true;
} else {
// Goal is not reachable. Trying to find nearest to the goal
// reachable point within its tolerance region
double best_sdist = std::numeric_limits<double>::max( );
p.position.y = goal.position.y - tolerance;
while ( p.position.y <= goal.position.y + tolerance ) {
p.position.x = goal.position.x - tolerance;
while ( p.position.x <= goal.position.x + tolerance ) {
potential = getPointPotential( p.position );
double sdist = squared_distance( p, goal );
if ( potential < POT_HIGH && sdist < best_sdist ) {
best_sdist = sdist;
best_pose = p;
found_legal = true;
}
p.position.x += resolution;
}
p.position.y += resolution;
}
}
if ( found_legal ) {
// extract the plan
if ( getPlanFromPotential( best_pose, plan ) ) {
smoothApproachToGoal( best_pose, plan );
// If use_final_approach_orientation=true, interpolate the last pose orientation from the
// previous pose to set the orientation to the 'final approach' orientation of the robot so
// it does not rotate.
// And deal with corner case of plan of length 1
if ( use_final_approach_orientation_ ) {
size_t plan_size = plan.poses.size( );
if ( plan_size == 1 ) {
plan.poses.back( ).pose.orientation = start.orientation;
} else if ( plan_size > 1 ) {
double dx, dy, theta;
auto last_pose = plan.poses.back( ).pose.position;
auto approach_pose = plan.poses[plan_size - 2].pose.position;
// Deal with the case of NavFn producing a path with two equal last poses
if ( std::abs( last_pose.x - approach_pose.x ) < 0.0001 &&
std::abs( last_pose.y - approach_pose.y ) < 0.0001 && plan_size > 2 )
{
approach_pose = plan.poses[plan_size - 3].pose.position;
}
dx = last_pose.x - approach_pose.x;
dy = last_pose.y - approach_pose.y;
theta = atan2( dy, dx );
plan.poses.back( ).pose.orientation =
nav2_util::geometry_utils::orientationAroundZAxis( theta );
}
}
} else {
RCLCPP_ERROR(
logger_,
"Failed to create a plan from potential when a legal"
" potential was found. This shouldn't happen." );
}
}
return !plan.poses.empty( );
}
void
352 NavfnPlanner::smoothApproachToGoal(
353 const geometry_msgs::msg::Pose & goal,
354 nav_msgs::msg::Path & plan )
{
// Replace the last pose of the computed path if it's actually further away
// to the second to last pose than the goal pose.
if ( plan.poses.size( ) >= 2 ) {
auto second_to_last_pose = plan.poses.end( )[-2];
auto last_pose = plan.poses.back( );
if (
squared_distance( last_pose.pose, second_to_last_pose.pose ) >
squared_distance( goal, second_to_last_pose.pose ) )
{
plan.poses.back( ).pose = goal;
return;
}
}
geometry_msgs::msg::PoseStamped goal_copy;
goal_copy.pose = goal;
plan.poses.push_back( goal_copy );
}
bool
375 NavfnPlanner::getPlanFromPotential(
376 const geometry_msgs::msg::Pose & goal,
377 nav_msgs::msg::Path & plan )
{
// clear the plan, just in case
plan.poses.clear( );
// Goal should be in global frame
double wx = goal.position.x;
double wy = goal.position.y;
// the potential has already been computed, so we won't update our copy of the costmap
unsigned int mx, my;
if ( !worldToMap( wx, wy, mx, my ) ) {
RCLCPP_WARN(
logger_,
"The goal sent to the navfn planner is off the global costmap."
" Planning will always fail to this goal." );
return false;
}
int map_goal[2];
map_goal[0] = mx;
map_goal[1] = my;
planner_->setStart( map_goal );
const int & max_cycles = ( costmap_->getSizeInCellsX( ) >= costmap_->getSizeInCellsY( ) ) ?
( costmap_->getSizeInCellsX( ) * 4 ) : ( costmap_->getSizeInCellsY( ) * 4 );
int path_len = planner_->calcPath( max_cycles );
if ( path_len == 0 ) {
return false;
}
auto cost = planner_->getLastPathCost( );
RCLCPP_DEBUG(
logger_,
"Path found, %d steps, %f cost\n", path_len, cost );
// extract the plan
float * x = planner_->getPathX( );
float * y = planner_->getPathY( );
int len = planner_->getPathLen( );
for ( int i = len - 1; i >= 0; --i ) {
// convert the plan to world coordinates
double world_x, world_y;
mapToWorld( x[i], y[i], world_x, world_y );
geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x = world_x;
pose.pose.position.y = world_y;
pose.pose.position.z = 0.0;
pose.pose.orientation.x = 0.0;
pose.pose.orientation.y = 0.0;
pose.pose.orientation.z = 0.0;
pose.pose.orientation.w = 1.0;
plan.poses.push_back( pose );
}
return !plan.poses.empty( );
}
double
440 NavfnPlanner::getPointPotential( const geometry_msgs::msg::Point & world_point )
{
unsigned int mx, my;
if ( !worldToMap( world_point.x, world_point.y, mx, my ) ) {
return std::numeric_limits<double>::max( );
}
unsigned int index = my * planner_->nx + mx;
return planner_->potarr[index];
}
// bool
// NavfnPlanner::validPointPotential( const geometry_msgs::msg::Point & world_point )
// {
// return validPointPotential( world_point, tolerance_ );
// }
// bool
// NavfnPlanner::validPointPotential(
// const geometry_msgs::msg::Point & world_point, double tolerance )
// {
// const double resolution = costmap_->getResolution( );
// geometry_msgs::msg::Point p = world_point;
// double potential = getPointPotential( p );
// if ( potential < POT_HIGH ) {
// // world_point is reachable by itself
// return true;
// } else {
// // world_point, is not reachable. Trying to find any
// // reachable point within its tolerance region
// p.y = world_point.y - tolerance;
// while ( p.y <= world_point.y + tolerance ) {
// p.x = world_point.x - tolerance;
// while ( p.x <= world_point.x + tolerance ) {
// potential = getPointPotential( p );
// if ( potential < POT_HIGH ) {
// return true;
// }
// p.x += resolution;
// }
// p.y += resolution;
// }
// }
// return false;
// }
bool
489 NavfnPlanner::worldToMap( double wx, double wy, unsigned int & mx, unsigned int & my )
{
if ( wx < costmap_->getOriginX( ) || wy < costmap_->getOriginY( ) ) {
return false;
}
mx = static_cast<int>(
std::round( ( wx - costmap_->getOriginX( ) ) / costmap_->getResolution( ) ) );
my = static_cast<int>(
std::round( ( wy - costmap_->getOriginY( ) ) / costmap_->getResolution( ) ) );
if ( mx < costmap_->getSizeInCellsX( ) && my < costmap_->getSizeInCellsY( ) ) {
return true;
}
RCLCPP_ERROR(
logger_,
"worldToMap failed: mx, my: %d, %d, size_x, size_y: %d, %d", mx, my,
costmap_->getSizeInCellsX( ), costmap_->getSizeInCellsY( ) );
return false;
}
void
513 NavfnPlanner::mapToWorld( double mx, double my, double & wx, double & wy )
{
wx = costmap_->getOriginX( ) + mx * costmap_->getResolution( );
wy = costmap_->getOriginY( ) + my * costmap_->getResolution( );
}
void
520 NavfnPlanner::clearRobotCell( unsigned int mx, unsigned int my )
{
// TODO( orduno ): check usage of this function, might instead be a request to
// world_model / map server
costmap_->setCost( mx, my, nav2_costmap_2d::FREE_SPACE );
}
rcl_interfaces::msg::SetParametersResult
528 NavfnPlanner::dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters )
{
rcl_interfaces::msg::SetParametersResult result;
for ( auto parameter : parameters ) {
const auto & type = parameter.get_type( );
const auto & name = parameter.get_name( );
if ( type == ParameterType::PARAMETER_DOUBLE ) {
if ( name == name_ + ".tolerance" ) {
tolerance_ = parameter.as_double( );
}
} else if ( type == ParameterType::PARAMETER_BOOL ) {
if ( name == name_ + ".use_astar" ) {
use_astar_ = parameter.as_bool( );
} else if ( name == name_ + ".allow_unknown" ) {
allow_unknown_ = parameter.as_bool( );
} else if ( name == name_ + ".use_final_approach_orientation" ) {
use_final_approach_orientation_ = parameter.as_bool( );
}
}
}
result.successful = true;
return result;
}
} // namespace nav2_navfn_planner
#include "pluginlib/class_list_macros.hpp"
556 PLUGINLIB_EXPORT_CLASS( nav2_navfn_planner::NavfnPlanner, nav2_core::GlobalPlanner )
1 // Copyright ( c ) 2021, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <math.h>
#include <memory>
#include <string>
#include <vector>
#include "gtest/gtest.h"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_navfn_planner/navfn_planner.hpp"
#include "rclcpp/rclcpp.hpp"
26 class RclCppFixture
{
public:
29 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
30 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
34 TEST( NavfnTest, testDynamicParameter )
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>( "Navfntest" );
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "global_costmap" );
costmap->on_configure( rclcpp_lifecycle::State( ) );
auto planner =
std::make_unique<nav2_navfn_planner::NavfnPlanner>( );
auto tf = std::make_shared<tf2_ros::Buffer>( node->get_clock( ) );
planner->configure( node, "test", tf, costmap );
planner->activate( );
auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
node->get_node_base_interface( ), node->get_node_topics_interface( ),
node->get_node_graph_interface( ),
node->get_node_services_interface( ) );
auto results = rec_param->set_parameters_atomically(
{rclcpp::Parameter( "test.tolerance", 1.0 ),
rclcpp::Parameter( "test.use_astar", true ),
rclcpp::Parameter( "test.allow_unknown", true ),
rclcpp::Parameter( "test.use_final_approach_orientation", true )} );
rclcpp::spin_until_future_complete(
node->get_node_base_interface( ),
results );
EXPECT_EQ( node->get_parameter( "test.tolerance" ).as_double( ), 1.0 );
EXPECT_EQ( node->get_parameter( "test.use_astar" ).as_bool( ), true );
EXPECT_EQ( node->get_parameter( "test.allow_unknown" ).as_bool( ), true );
EXPECT_EQ( node->get_parameter( "test.use_final_approach_orientation" ).as_bool( ), true );
}
// Copyright ( c ) 2019 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_PLANNER__PLANNER_SERVER_HPP_
#define NAV2_PLANNER__PLANNER_SERVER_HPP_
#include <chrono>
#include <string>
#include <memory>
#include <vector>
#include <unordered_map>
#include <mutex>
#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav_msgs/msg/path.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_msgs/action/compute_path_to_pose.hpp"
#include "nav2_msgs/action/compute_path_through_poses.hpp"
#include "nav2_msgs/msg/costmap.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav2_util/simple_action_server.hpp"
#include "visualization_msgs/msg/marker.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/create_timer_ros.h"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "pluginlib/class_loader.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "nav2_core/global_planner.hpp"
#include "nav2_msgs/srv/is_path_valid.hpp"
namespace nav2_planner
{
/**
* @class nav2_planner::PlannerServer
* @brief An action server implements the behavior tree's ComputePathToPose
* interface and hosts various plugins of different algorithms to compute plans.
*/
50 class PlannerServer : public nav2_util::LifecycleNode
{
public:
/**
* @brief A constructor for nav2_planner::PlannerServer
* @param options Additional options to control creation of the node.
*/
57 explicit PlannerServer( const rclcpp::NodeOptions & options = rclcpp::NodeOptions( ) );
/**
* @brief A destructor for nav2_planner::PlannerServer
*/
61 ~PlannerServer( );
using PlannerMap = std::unordered_map<std::string, nav2_core::GlobalPlanner::Ptr>;
/**
* @brief Method to get plan from the desired plugin
* @param start starting pose
* @param goal goal request
* @return Path
*/
nav_msgs::msg::Path getPlan(
const geometry_msgs::msg::PoseStamped & start,
const geometry_msgs::msg::PoseStamped & goal,
const std::string & planner_id );
protected:
/**
* @brief Configure member variables and initializes planner
* @param state Reference to LifeCycle node state
* @return SUCCESS or FAILURE
*/
nav2_util::CallbackReturn on_configure( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Activate member variables
* @param state Reference to LifeCycle node state
* @return SUCCESS or FAILURE
*/
nav2_util::CallbackReturn on_activate( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Deactivate member variables
* @param state Reference to LifeCycle node state
* @return SUCCESS or FAILURE
*/
nav2_util::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Reset member variables
* @param state Reference to LifeCycle node state
* @return SUCCESS or FAILURE
*/
nav2_util::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Called when in shutdown state
* @param state Reference to LifeCycle node state
* @return SUCCESS or FAILURE
*/
nav2_util::CallbackReturn on_shutdown( const rclcpp_lifecycle::State & state ) override;
using ActionToPose = nav2_msgs::action::ComputePathToPose;
using ActionThroughPoses = nav2_msgs::action::ComputePathThroughPoses;
using ActionServerToPose = nav2_util::SimpleActionServer<ActionToPose>;
using ActionServerThroughPoses = nav2_util::SimpleActionServer<ActionThroughPoses>;
/**
* @brief Check if an action server is valid / active
* @param action_server Action server to test
* @return SUCCESS or FAILURE
*/
template<typename T>
bool isServerInactive( std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server );
/**
* @brief Check if an action server has a cancellation request pending
* @param action_server Action server to test
* @return SUCCESS or FAILURE
*/
template<typename T>
bool isCancelRequested( std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server );
/**
* @brief Wait for costmap to be valid with updated sensor data or repopulate after a
* clearing recovery. Blocks until true without timeout.
*/
void waitForCostmap( );
/**
* @brief Check if an action server has a preemption request and replaces the goal
* with the new preemption goal.
* @param action_server Action server to get updated goal if required
* @param goal Goal to overwrite
*/
template<typename T>
void getPreemptedGoalIfRequested(
std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,
typename std::shared_ptr<const typename T::Goal> goal );
/**
* @brief Get the starting pose from costmap or message, if valid
* @param action_server Action server to terminate if required
* @param goal Goal to find start from
* @param start The starting pose to use
* @return bool If successful in finding a valid starting pose
*/
template<typename T>
bool getStartPose(
std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,
typename std::shared_ptr<const typename T::Goal> goal,
geometry_msgs::msg::PoseStamped & start );
/**
* @brief Transform start and goal poses into the costmap
* global frame for path planning plugins to utilize
* @param action_server Action server to terminate if required
* @param start The starting pose to transform
* @param goal Goal pose to transform
* @return bool If successful in transforming poses
*/
template<typename T>
bool transformPosesToGlobalFrame(
std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,
geometry_msgs::msg::PoseStamped & curr_start,
geometry_msgs::msg::PoseStamped & curr_goal );
/**
* @brief Validate that the path contains a meaningful path
* @param action_server Action server to terminate if required
* @param goal Goal Current goal
* @param path Current path
* @param planner_id The planner ID used to generate the path
* @return bool If path is valid
*/
template<typename T>
bool validatePath(
std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,
const geometry_msgs::msg::PoseStamped & curr_goal,
const nav_msgs::msg::Path & path,
const std::string & planner_id );
// Our action server implements the ComputePathToPose action
std::unique_ptr<ActionServerToPose> action_server_pose_;
std::unique_ptr<ActionServerThroughPoses> action_server_poses_;
/**
* @brief The action server callback which calls planner to get the path
* ComputePathToPose
*/
void computePlan( );
/**
* @brief The action server callback which calls planner to get the path
* ComputePathThroughPoses
*/
void computePlanThroughPoses( );
/**
* @brief The service callback to determine if the path is still valid
* @param request to the service
* @param response from the service
*/
void isPathValid(
const std::shared_ptr<nav2_msgs::srv::IsPathValid::Request> request,
std::shared_ptr<nav2_msgs::srv::IsPathValid::Response> response );
/**
* @brief Publish a path for visualization purposes
* @param path Reference to Global Path
*/
void publishPlan( const nav_msgs::msg::Path & path );
/**
* @brief Callback executed when a parameter change is detected
* @param event ParameterEvent message
*/
rcl_interfaces::msg::SetParametersResult
dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters );
// Dynamic parameters handler
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
std::mutex dynamic_params_lock_;
// Planner
PlannerMap planners_;
pluginlib::ClassLoader<nav2_core::GlobalPlanner> gp_loader_;
std::vector<std::string> default_ids_;
std::vector<std::string> default_types_;
std::vector<std::string> planner_ids_;
std::vector<std::string> planner_types_;
double max_planner_duration_;
std::string planner_ids_concat_;
// Clock
rclcpp::Clock steady_clock_{RCL_STEADY_TIME};
// TF buffer
std::shared_ptr<tf2_ros::Buffer> tf_;
// Global Costmap
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
std::unique_ptr<nav2_util::NodeThread> costmap_thread_;
nav2_costmap_2d::Costmap2D * costmap_;
// Publishers for the path
rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr plan_publisher_;
// Service to deterime if the path is valid
rclcpp::Service<nav2_msgs::srv::IsPathValid>::SharedPtr is_path_valid_service_;
};
} // namespace nav2_planner
#endif // NAV2_PLANNER__PLANNER_SERVER_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2019 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include "nav2_planner/planner_server.hpp"
#include "rclcpp/rclcpp.hpp"
21 int main( int argc, char ** argv )
{
rclcpp::init( argc, argv );
auto node = std::make_shared<nav2_planner::PlannerServer>( );
rclcpp::spin( node->get_node_base_interface( ) );
rclcpp::shutdown( );
return 0;
}
// Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2019 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <chrono>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <limits>
#include <iterator>
#include <memory>
#include <string>
#include <vector>
#include <utility>
#include "builtin_interfaces/msg/duration.hpp"
#include "nav2_util/costmap.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_costmap_2d/cost_values.hpp"
#include "nav2_planner/planner_server.hpp"
using namespace std::chrono_literals;
using rcl_interfaces::msg::ParameterType;
using std::placeholders::_1;
namespace nav2_planner
{
42 PlannerServer::PlannerServer( const rclcpp::NodeOptions & options )
: nav2_util::LifecycleNode( "planner_server", "", options ),
gp_loader_( "nav2_core", "nav2_core::GlobalPlanner" ),
default_ids_{"GridBased"},
46 default_types_{"nav2_navfn_planner/NavfnPlanner"},
47 costmap_( nullptr )
{
RCLCPP_INFO( get_logger( ), "Creating" );
// Declare this node's parameters
declare_parameter( "planner_plugins", default_ids_ );
declare_parameter( "expected_planner_frequency", 1.0 );
get_parameter( "planner_plugins", planner_ids_ );
if ( planner_ids_ == default_ids_ ) {
for ( size_t i = 0; i < default_ids_.size( ); ++i ) {
declare_parameter( default_ids_[i] + ".plugin", default_types_[i] );
}
}
// Setup the global costmap
costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
"global_costmap", std::string{get_namespace( )}, "global_costmap" );
// Launch a thread to run the costmap node
costmap_thread_ = std::make_unique<nav2_util::NodeThread>( costmap_ros_ );
}
70 PlannerServer::~PlannerServer( )
{
planners_.clear( );
costmap_thread_.reset( );
}
nav2_util::CallbackReturn
77 PlannerServer::on_configure( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Configuring" );
costmap_ros_->configure( );
costmap_ = costmap_ros_->getCostmap( );
RCLCPP_DEBUG(
get_logger( ), "Costmap size: %d, %d",
costmap_->getSizeInCellsX( ), costmap_->getSizeInCellsY( ) );
tf_ = costmap_ros_->getTfBuffer( );
planner_types_.resize( planner_ids_.size( ) );
auto node = shared_from_this( );
for ( size_t i = 0; i != planner_ids_.size( ); i++ ) {
try {
planner_types_[i] = nav2_util::get_plugin_type_param(
node, planner_ids_[i] );
nav2_core::GlobalPlanner::Ptr planner =
gp_loader_.createUniqueInstance( planner_types_[i] );
RCLCPP_INFO(
get_logger( ), "Created global planner plugin %s of type %s",
planner_ids_[i].c_str( ), planner_types_[i].c_str( ) );
planner->configure( node, planner_ids_[i], tf_, costmap_ros_ );
planners_.insert( {planner_ids_[i], planner} );
} catch ( const pluginlib::PluginlibException & ex ) {
RCLCPP_FATAL(
get_logger( ), "Failed to create global planner. Exception: %s",
ex.what( ) );
return nav2_util::CallbackReturn::FAILURE;
}
}
for ( size_t i = 0; i != planner_ids_.size( ); i++ ) {
planner_ids_concat_ += planner_ids_[i] + std::string( " " );
}
RCLCPP_INFO(
get_logger( ),
"Planner Server has %s planners available.", planner_ids_concat_.c_str( ) );
double expected_planner_frequency;
get_parameter( "expected_planner_frequency", expected_planner_frequency );
if ( expected_planner_frequency > 0 ) {
max_planner_duration_ = 1 / expected_planner_frequency;
} else {
RCLCPP_WARN(
get_logger( ),
"The expected planner frequency parameter is %.4f Hz. The value should to be greater"
" than 0.0 to turn on duration overrrun warning messages", expected_planner_frequency );
max_planner_duration_ = 0.0;
}
// Initialize pubs & subs
plan_publisher_ = create_publisher<nav_msgs::msg::Path>( "plan", 1 );
// Create the action servers for path planning to a pose and through poses
action_server_pose_ = std::make_unique<ActionServerToPose>(
shared_from_this( ),
"compute_path_to_pose",
std::bind( &PlannerServer::computePlan, this ),
nullptr,
std::chrono::milliseconds( 500 ),
true );
action_server_poses_ = std::make_unique<ActionServerThroughPoses>(
shared_from_this( ),
"compute_path_through_poses",
std::bind( &PlannerServer::computePlanThroughPoses, this ),
nullptr,
std::chrono::milliseconds( 500 ),
true );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
157 PlannerServer::on_activate( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Activating" );
plan_publisher_->on_activate( );
action_server_pose_->activate( );
action_server_poses_->activate( );
costmap_ros_->activate( );
PlannerMap::iterator it;
for ( it = planners_.begin( ); it != planners_.end( ); ++it ) {
it->second->activate( );
}
auto node = shared_from_this( );
is_path_valid_service_ = node->create_service<nav2_msgs::srv::IsPathValid>(
"is_path_valid",
std::bind(
&PlannerServer::isPathValid, this,
std::placeholders::_1, std::placeholders::_2 ) );
// Add callback for dynamic parameters
dyn_params_handler_ = node->add_on_set_parameters_callback(
std::bind( &PlannerServer::dynamicParametersCallback, this, _1 ) );
// create bond connection
createBond( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
190 PlannerServer::on_deactivate( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Deactivating" );
action_server_pose_->deactivate( );
action_server_poses_->deactivate( );
plan_publisher_->on_deactivate( );
costmap_ros_->deactivate( );
PlannerMap::iterator it;
for ( it = planners_.begin( ); it != planners_.end( ); ++it ) {
it->second->deactivate( );
}
dyn_params_handler_.reset( );
// destroy bond connection
destroyBond( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
213 PlannerServer::on_cleanup( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Cleaning up" );
action_server_pose_.reset( );
action_server_poses_.reset( );
plan_publisher_.reset( );
tf_.reset( );
costmap_ros_->cleanup( );
PlannerMap::iterator it;
for ( it = planners_.begin( ); it != planners_.end( ); ++it ) {
it->second->cleanup( );
}
planners_.clear( );
costmap_ = nullptr;
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
233 PlannerServer::on_shutdown( const rclcpp_lifecycle::State & )
{
RCLCPP_INFO( get_logger( ), "Shutting down" );
return nav2_util::CallbackReturn::SUCCESS;
}
template<typename T>
240 bool PlannerServer::isServerInactive(
std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server )
{
if ( action_server == nullptr || !action_server->is_server_active( ) ) {
RCLCPP_DEBUG( get_logger( ), "Action server unavailable or inactive. Stopping." );
return true;
}
return false;
}
251 void PlannerServer::waitForCostmap( )
{
// Don't compute a plan until costmap is valid ( after clear costmap )
rclcpp::Rate r( 100 );
while ( !costmap_ros_->isCurrent( ) ) {
r.sleep( );
}
}
template<typename T>
261 bool PlannerServer::isCancelRequested(
std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server )
{
if ( action_server->is_cancel_requested( ) ) {
RCLCPP_INFO( get_logger( ), "Goal was canceled. Canceling planning action." );
action_server->terminate_all( );
return true;
}
return false;
}
template<typename T>
274 void PlannerServer::getPreemptedGoalIfRequested(
std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,
typename std::shared_ptr<const typename T::Goal> goal )
{
if ( action_server->is_preempt_requested( ) ) {
goal = action_server->accept_pending_goal( );
}
}
template<typename T>
284 bool PlannerServer::getStartPose(
std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,
typename std::shared_ptr<const typename T::Goal> goal,
geometry_msgs::msg::PoseStamped & start )
{
if ( goal->use_start ) {
start = goal->start;
} else if ( !costmap_ros_->getRobotPose( start ) ) {
action_server->terminate_current( );
return false;
}
return true;
}
template<typename T>
300 bool PlannerServer::transformPosesToGlobalFrame(
std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,
geometry_msgs::msg::PoseStamped & curr_start,
geometry_msgs::msg::PoseStamped & curr_goal )
{
if ( !costmap_ros_->transformPoseToGlobalFrame( curr_start, curr_start ) ||
!costmap_ros_->transformPoseToGlobalFrame( curr_goal, curr_goal ) )
{
RCLCPP_WARN(
get_logger( ), "Could not transform the start or goal pose in the costmap frame" );
action_server->terminate_current( );
return false;
}
return true;
}
template<typename T>
318 bool PlannerServer::validatePath(
319 std::unique_ptr<nav2_util::SimpleActionServer<T>> & action_server,
const geometry_msgs::msg::PoseStamped & goal,
const nav_msgs::msg::Path & path,
const std::string & planner_id )
{
if ( path.poses.size( ) == 0 ) {
RCLCPP_WARN(
get_logger( ), "Planning algorithm %s failed to generate a valid"
" path to ( %.2f, %.2f )", planner_id.c_str( ),
goal.pose.position.x, goal.pose.position.y );
action_server->terminate_current( );
return false;
}
RCLCPP_DEBUG(
get_logger( ),
"Found valid path of size %zu to ( %.2f, %.2f )",
path.poses.size( ), goal.pose.position.x,
goal.pose.position.y );
return true;
}
void
PlannerServer::computePlanThroughPoses( )
{
std::lock_guard<std::mutex> lock( dynamic_params_lock_ );
auto start_time = steady_clock_.now( );
// Initialize the ComputePathToPose goal and result
auto goal = action_server_poses_->get_current_goal( );
auto result = std::make_shared<ActionThroughPoses::Result>( );
nav_msgs::msg::Path concat_path;
try {
if ( isServerInactive( action_server_poses_ ) || isCancelRequested( action_server_poses_ ) ) {
return;
}
waitForCostmap( );
getPreemptedGoalIfRequested( action_server_poses_, goal );
if ( goal->goals.size( ) == 0 ) {
RCLCPP_WARN(
get_logger( ),
"Compute path through poses requested a plan with no viapoint poses, returning." );
action_server_poses_->terminate_current( );
}
// Use start pose if provided otherwise use current robot pose
geometry_msgs::msg::PoseStamped start;
if ( !getStartPose( action_server_poses_, goal, start ) ) {
return;
}
// Get consecutive paths through these points
std::vector<geometry_msgs::msg::PoseStamped>::iterator goal_iter;
geometry_msgs::msg::PoseStamped curr_start, curr_goal;
for ( unsigned int i = 0; i != goal->goals.size( ); i++ ) {
// Get starting point
if ( i == 0 ) {
curr_start = start;
} else {
curr_start = goal->goals[i - 1];
}
curr_goal = goal->goals[i];
// Transform them into the global frame
if ( !transformPosesToGlobalFrame( action_server_poses_, curr_start, curr_goal ) ) {
return;
}
// Get plan from start -> goal
nav_msgs::msg::Path curr_path = getPlan( curr_start, curr_goal, goal->planner_id );
// check path for validity
if ( !validatePath( action_server_poses_, curr_goal, curr_path, goal->planner_id ) ) {
return;
}
// Concatenate paths together
concat_path.poses.insert(
concat_path.poses.end( ), curr_path.poses.begin( ), curr_path.poses.end( ) );
concat_path.header = curr_path.header;
}
// Publish the plan for visualization purposes
result->path = concat_path;
publishPlan( result->path );
auto cycle_duration = steady_clock_.now( ) - start_time;
result->planning_time = cycle_duration;
if ( max_planner_duration_ && cycle_duration.seconds( ) > max_planner_duration_ ) {
RCLCPP_WARN(
get_logger( ),
"Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz",
1 / max_planner_duration_, 1 / cycle_duration.seconds( ) );
}
action_server_poses_->succeeded_current( result );
} catch ( std::exception & ex ) {
RCLCPP_WARN(
get_logger( ),
"%s plugin failed to plan through %zu points with final goal ( %.2f, %.2f ): \"%s\"",
goal->planner_id.c_str( ), goal->goals.size( ), goal->goals.back( ).pose.position.x,
goal->goals.back( ).pose.position.y, ex.what( ) );
action_server_poses_->terminate_current( );
}
}
void
PlannerServer::computePlan( )
{
std::lock_guard<std::mutex> lock( dynamic_params_lock_ );
auto start_time = steady_clock_.now( );
// Initialize the ComputePathToPose goal and result
auto goal = action_server_pose_->get_current_goal( );
auto result = std::make_shared<ActionToPose::Result>( );
try {
if ( isServerInactive( action_server_pose_ ) || isCancelRequested( action_server_pose_ ) ) {
return;
}
waitForCostmap( );
getPreemptedGoalIfRequested( action_server_pose_, goal );
// Use start pose if provided otherwise use current robot pose
geometry_msgs::msg::PoseStamped start;
if ( !getStartPose( action_server_pose_, goal, start ) ) {
return;
}
// Transform them into the global frame
geometry_msgs::msg::PoseStamped goal_pose = goal->goal;
if ( !transformPosesToGlobalFrame( action_server_pose_, start, goal_pose ) ) {
return;
}
result->path = getPlan( start, goal_pose, goal->planner_id );
if ( !validatePath( action_server_pose_, goal_pose, result->path, goal->planner_id ) ) {
return;
}
// Publish the plan for visualization purposes
publishPlan( result->path );
auto cycle_duration = steady_clock_.now( ) - start_time;
result->planning_time = cycle_duration;
if ( max_planner_duration_ && cycle_duration.seconds( ) > max_planner_duration_ ) {
RCLCPP_WARN(
get_logger( ),
"Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz",
1 / max_planner_duration_, 1 / cycle_duration.seconds( ) );
}
483 action_server_pose_->succeeded_current( result );
} catch ( std::exception & ex ) {
RCLCPP_WARN(
get_logger( ), "%s plugin failed to plan calculation to ( %.2f, %.2f ): \"%s\"",
goal->planner_id.c_str( ), goal->goal.pose.position.x,
goal->goal.pose.position.y, ex.what( ) );
action_server_pose_->terminate_current( );
}
}
nav_msgs::msg::Path
PlannerServer::getPlan(
const geometry_msgs::msg::PoseStamped & start,
const geometry_msgs::msg::PoseStamped & goal,
const std::string & planner_id )
{
RCLCPP_DEBUG(
get_logger( ), "Attempting to a find path from ( %.2f, %.2f ) to "
"( %.2f, %.2f ).", start.pose.position.x, start.pose.position.y,
goal.pose.position.x, goal.pose.position.y );
if ( planners_.find( planner_id ) != planners_.end( ) ) {
return planners_[planner_id]->createPlan( start, goal );
} else {
if ( planners_.size( ) == 1 && planner_id.empty( ) ) {
RCLCPP_WARN_ONCE(
get_logger( ), "No planners specified in action call. "
"Server will use only plugin %s in server."
" This warning will appear once.", planner_ids_concat_.c_str( ) );
return planners_[planners_.begin( )->first]->createPlan( start, goal );
} else {
RCLCPP_ERROR(
get_logger( ), "planner %s is not a valid planner. "
"Planner names are: %s", planner_id.c_str( ),
planner_ids_concat_.c_str( ) );
}
}
return nav_msgs::msg::Path( );
}
void
PlannerServer::publishPlan( const nav_msgs::msg::Path & path )
{
auto msg = std::make_unique<nav_msgs::msg::Path>( path );
if ( plan_publisher_->is_activated( ) && plan_publisher_->get_subscription_count( ) > 0 ) {
plan_publisher_->publish( std::move( msg ) );
}
}
void PlannerServer::isPathValid(
const std::shared_ptr<nav2_msgs::srv::IsPathValid::Request> request,
std::shared_ptr<nav2_msgs::srv::IsPathValid::Response> response )
{
response->is_valid = true;
if ( request->path.poses.empty( ) ) {
response->is_valid = false;
return;
}
geometry_msgs::msg::PoseStamped current_pose;
unsigned int closest_point_index = 0;
if ( costmap_ros_->getRobotPose( current_pose ) ) {
float current_distance = std::numeric_limits<float>::max( );
float closest_distance = current_distance;
geometry_msgs::msg::Point current_point = current_pose.pose.position;
for ( unsigned int i = 0; i < request->path.poses.size( ); ++i ) {
geometry_msgs::msg::Point path_point = request->path.poses[i].pose.position;
current_distance = nav2_util::geometry_utils::euclidean_distance(
current_point,
path_point );
if ( current_distance < closest_distance ) {
closest_point_index = i;
closest_distance = current_distance;
}
}
/**
* The lethal check starts at the closest point to avoid points that have already been passed
* and may have become occupied
*/
unsigned int mx = 0;
unsigned int my = 0;
for ( unsigned int i = closest_point_index; i < request->path.poses.size( ); ++i ) {
costmap_->worldToMap(
request->path.poses[i].pose.position.x,
request->path.poses[i].pose.position.y, mx, my );
unsigned int cost = costmap_->getCost( mx, my );
if ( cost == nav2_costmap_2d::LETHAL_OBSTACLE ||
cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE )
{
response->is_valid = false;
}
}
}
}
rcl_interfaces::msg::SetParametersResult
PlannerServer::dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters )
{
std::lock_guard<std::mutex> lock( dynamic_params_lock_ );
rcl_interfaces::msg::SetParametersResult result;
for ( auto parameter : parameters ) {
const auto & type = parameter.get_type( );
const auto & name = parameter.get_name( );
if ( type == ParameterType::PARAMETER_DOUBLE ) {
if ( name == "expected_planner_frequency" ) {
if ( parameter.as_double( ) > 0 ) {
max_planner_duration_ = 1 / parameter.as_double( );
} else {
RCLCPP_WARN(
get_logger( ),
"The expected planner frequency parameter is %.4f Hz. The value should to be greater"
" than 0.0 to turn on duration overrrun warning messages", parameter.as_double( ) );
max_planner_duration_ = 0.0;
}
}
}
}
result.successful = true;
return result;
}
} // namespace nav2_planner
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE( nav2_planner::PlannerServer )
1 // Copyright ( c ) 2021, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <math.h>
#include <memory>
#include <string>
#include <vector>
#include "gtest/gtest.h"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_planner/planner_server.hpp"
#include "rclcpp/rclcpp.hpp"
26 class PlannerShim : public nav2_planner::PlannerServer
{
public:
29 PlannerShim( )
: nav2_planner::PlannerServer( rclcpp::NodeOptions( ) )
{
}
// Since we cannot call configure/activate due to costmaps
// requiring TF
36 void setDynamicCallback( )
{
auto node = shared_from_this( );
// Add callback for dynamic parameters
dyn_params_handler_ = node->add_on_set_parameters_callback(
std::bind( &PlannerShim::dynamicParamsShim, this, std::placeholders::_1 ) );
}
rcl_interfaces::msg::SetParametersResult
45 dynamicParamsShim( std::vector<rclcpp::Parameter> parameters )
{
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
dynamicParametersCallback( parameters );
return result;
}
};
54 class RclCppFixture
{
public:
57 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
58 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
62 TEST( WPTest, test_dynamic_parameters )
{
auto planner = std::make_shared<PlannerShim>( );
planner->setDynamicCallback( );
auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
planner->get_node_base_interface( ), planner->get_node_topics_interface( ),
planner->get_node_graph_interface( ),
planner->get_node_services_interface( ) );
auto results = rec_param->set_parameters_atomically(
{rclcpp::Parameter( "expected_planner_frequency", 100.0 )} );
rclcpp::spin_until_future_complete(
planner->get_node_base_interface( ),
results );
EXPECT_EQ( planner->get_parameter( "expected_planner_frequency" ).as_double( ), 100.0 );
// test edge case for = 0
results = rec_param->set_parameters_atomically(
{rclcpp::Parameter( "expected_planner_frequency", -1.0 )} );
rclcpp::spin_until_future_complete(
planner->get_node_base_interface( ),
results );
EXPECT_EQ( planner->get_parameter( "expected_planner_frequency" ).as_double( ), -1.0 );
}
// Copyright ( c ) 2020 Shrijit Singh
// Copyright ( c ) 2020 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__REGULATED_PURE_PURSUIT_CONTROLLER_HPP_
#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__REGULATED_PURE_PURSUIT_CONTROLLER_HPP_
#include <string>
#include <vector>
#include <memory>
#include <algorithm>
#include <mutex>
#include "nav2_costmap_2d/footprint_collision_checker.hpp"
#include "nav2_core/controller.hpp"
#include "rclcpp/rclcpp.hpp"
#include "pluginlib/class_loader.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "nav2_util/odometry_utils.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "geometry_msgs/msg/pose2_d.hpp"
namespace nav2_regulated_pure_pursuit_controller
{
/**
* @class nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
* @brief Regulated pure pursuit controller plugin
*/
41 class RegulatedPurePursuitController : public nav2_core::Controller
{
public:
/**
* @brief Constructor for nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
*/
RegulatedPurePursuitController( ) = default;
/**
* @brief Destrructor for nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
*/
~RegulatedPurePursuitController( ) override = default;
/**
* @brief Configure controller state machine
* @param parent WeakPtr to node
* @param name Name of plugin
* @param tf TF buffer
* @param costmap_ros Costmap2DROS object of environment
*/
void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros ) override;
/**
* @brief Cleanup controller state machine
*/
void cleanup( ) override;
/**
* @brief Activate controller state machine
*/
void activate( ) override;
/**
* @brief Deactivate controller state machine
*/
void deactivate( ) override;
/**
* @brief Compute the best command given the current pose and velocity, with possible debug information
*
* Same as above computeVelocityCommands, but with debug results.
* If the results pointer is not null, additional information about the twists
* evaluated will be in results after the call.
*
* @param pose Current robot pose
* @param velocity Current robot velocity
* @param goal_checker Ptr to the goal checker for this task in case useful in computing commands
* @return Best command
*/
geometry_msgs::msg::TwistStamped computeVelocityCommands(
const geometry_msgs::msg::PoseStamped & pose,
const geometry_msgs::msg::Twist & velocity,
nav2_core::GoalChecker * /*goal_checker*/ ) override;
/**
* @brief nav2_core setPlan - Sets the global plan
* @param path The global plan
*/
void setPlan( const nav_msgs::msg::Path & path ) override;
/**
* @brief Limits the maximum linear speed of the robot.
* @param speed_limit expressed in absolute value ( in m/s )
* or in percentage from maximum robot speed.
* @param percentage Setting speed limit in percentage if true
* or in absolute values in false case.
*/
void setSpeedLimit( const double & speed_limit, const bool & percentage ) override;
protected:
/**
* @brief Transforms global plan into same frame as pose and clips poses ineligible for lookaheadPoint
* Points ineligible to be selected as a lookahead point if they are any of the following:
* - Outside the local_costmap ( collision avoidance cannot be assured )
* @param pose pose to transform
* @return Path in new frame
*/
nav_msgs::msg::Path transformGlobalPlan(
const geometry_msgs::msg::PoseStamped & pose );
/**
* @brief Transform a pose to another frame.
* @param frame Frame ID to transform to
* @param in_pose Pose input to transform
* @param out_pose transformed output
* @return bool if successful
*/
bool transformPose(
const std::string frame,
const geometry_msgs::msg::PoseStamped & in_pose,
geometry_msgs::msg::PoseStamped & out_pose ) const;
/**
* @brief Get lookahead distance
* @param cmd the current speed to use to compute lookahead point
* @return lookahead distance
*/
double getLookAheadDistance( const geometry_msgs::msg::Twist & );
/**
* @brief Creates a PointStamped message for visualization
* @param carrot_pose Input carrot point as a PoseStamped
* @return CarrotMsg a carrot point marker, PointStamped
*/
std::unique_ptr<geometry_msgs::msg::PointStamped> createCarrotMsg(
const geometry_msgs::msg::PoseStamped & carrot_pose );
/**
* @brief Whether robot should rotate to rough path heading
* @param carrot_pose current lookahead point
* @param angle_to_path Angle of robot output relatie to carrot marker
* @return Whether should rotate to path heading
*/
bool shouldRotateToPath(
const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path );
/**
* @brief Whether robot should rotate to final goal orientation
* @param carrot_pose current lookahead point
* @return Whether should rotate to goal heading
*/
bool shouldRotateToGoalHeading( const geometry_msgs::msg::PoseStamped & carrot_pose );
/**
* @brief Create a smooth and kinematically smoothed rotation command
* @param linear_vel linear velocity
* @param angular_vel angular velocity
* @param angle_to_path Angle of robot output relatie to carrot marker
* @param curr_speed the current robot speed
*/
void rotateToHeading(
double & linear_vel, double & angular_vel,
const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed );
/**
* @brief Whether collision is imminent
* @param robot_pose Pose of robot
* @param carrot_pose Pose of carrot
* @param linear_vel linear velocity to forward project
* @param angular_vel angular velocity to forward project
* @param carrot_dist Distance to the carrot for PP
* @return Whether collision is imminent
*/
bool isCollisionImminent(
const geometry_msgs::msg::PoseStamped &,
const double &, const double &,
const double & );
/**
* @brief checks for collision at projected pose
* @param x Pose of pose x
* @param y Pose of pose y
* @param theta orientation of Yaw
* @return Whether in collision
*/
bool inCollision(
const double & x,
const double & y,
const double & theta );
/**
* @brief Cost at a point
* @param x Pose of pose x
* @param y Pose of pose y
* @return Cost of pose in costmap
*/
double costAtPose( const double & x, const double & y );
/**
* @brief apply regulation constraints to the system
* @param linear_vel robot command linear velocity input
* @param dist_error error in the carrot distance and lookahead distance
* @param lookahead_dist optimal lookahead distance
* @param curvature curvature of path
* @param speed Speed of robot
* @param pose_cost cost at this pose
*/
void applyConstraints(
const double & dist_error, const double & lookahead_dist,
const double & curvature, const geometry_msgs::msg::Twist & speed,
const double & pose_cost, double & linear_vel, double & sign );
/**
* @brief Find the intersection a circle and a line segment.
* This assumes the circle is centered at the origin.
* If no intersection is found, a floating point error will occur.
* @param p1 first endpoint of line segment
* @param p2 second endpoint of line segment
* @param r radius of circle
* @return point of intersection
*/
static geometry_msgs::msg::Point circleSegmentIntersection(
const geometry_msgs::msg::Point & p1,
const geometry_msgs::msg::Point & p2,
double r );
/**
* @brief Get lookahead point
* @param lookahead_dist Optimal lookahead distance
* @param path Current global path
* @return Lookahead point
*/
geometry_msgs::msg::PoseStamped getLookAheadPoint( const double &, const nav_msgs::msg::Path & );
/**
* @brief checks for the cusp position
* @param pose Pose input to determine the cusp position
* @return robot distance from the cusp
*/
double findVelocitySignChange( const nav_msgs::msg::Path & transformed_plan );
/**
* Get the greatest extent of the costmap in meters from the center.
* @return max of distance from center in meters to edge of costmap
*/
double getCostmapMaxExtent( ) const;
/**
* @brief Callback executed when a parameter change is detected
* @param event ParameterEvent message
*/
rcl_interfaces::msg::SetParametersResult
dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters );
rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
std::shared_ptr<tf2_ros::Buffer> tf_;
std::string plugin_name_;
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
nav2_costmap_2d::Costmap2D * costmap_;
rclcpp::Logger logger_ {rclcpp::get_logger( "RegulatedPurePursuitController" )};
rclcpp::Clock::SharedPtr clock_;
double desired_linear_vel_, base_desired_linear_vel_;
double lookahead_dist_;
double rotate_to_heading_angular_vel_;
double max_lookahead_dist_;
double min_lookahead_dist_;
double lookahead_time_;
bool use_velocity_scaled_lookahead_dist_;
tf2::Duration transform_tolerance_;
double min_approach_linear_velocity_;
double control_duration_;
double max_allowed_time_to_collision_up_to_carrot_;
bool use_regulated_linear_velocity_scaling_;
bool use_cost_regulated_linear_velocity_scaling_;
double cost_scaling_dist_;
double cost_scaling_gain_;
double inflation_cost_scaling_factor_;
double regulated_linear_scaling_min_radius_;
double regulated_linear_scaling_min_speed_;
bool use_rotate_to_heading_;
double max_angular_accel_;
double rotate_to_heading_min_angle_;
double goal_dist_tol_;
bool allow_reversing_;
double max_robot_pose_search_dist_;
bool use_interpolation_;
nav_msgs::msg::Path global_plan_;
302 std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> global_path_pub_;
303 std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PointStamped>>
carrot_pub_;
305 std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> carrot_arc_pub_;
306 std::unique_ptr<nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>
collision_checker_;
// Dynamic parameters handler
std::mutex mutex_;
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
};
} // namespace nav2_regulated_pure_pursuit_controller
#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__REGULATED_PURE_PURSUIT_CONTROLLER_HPP_
1 // Copyright ( c ) 2020 Shrijit Singh
// Copyright ( c ) 2020 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <algorithm>
#include <string>
#include <limits>
#include <memory>
#include <vector>
#include <utility>
#include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp"
#include "nav2_core/exceptions.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
using std::hypot;
using std::min;
using std::max;
using std::abs;
using nav2_util::declare_parameter_if_not_declared;
using nav2_util::geometry_utils::euclidean_distance;
using namespace nav2_costmap_2d; // NOLINT
using rcl_interfaces::msg::ParameterType;
namespace nav2_regulated_pure_pursuit_controller
{
41 void RegulatedPurePursuitController::configure(
42 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
43 std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
44 std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros )
{
auto node = parent.lock( );
node_ = parent;
if ( !node ) {
throw nav2_core::PlannerException( "Unable to lock node!" );
}
costmap_ros_ = costmap_ros;
costmap_ = costmap_ros_->getCostmap( );
tf_ = tf;
plugin_name_ = name;
logger_ = node->get_logger( );
clock_ = node->get_clock( );
double transform_tolerance = 0.1;
double control_frequency = 20.0;
goal_dist_tol_ = 0.25; // reasonable default before first update
declare_parameter_if_not_declared(
node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue( 0.5 ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue( 0.6 ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".min_lookahead_dist", rclcpp::ParameterValue( 0.3 ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".max_lookahead_dist", rclcpp::ParameterValue( 0.9 ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".lookahead_time", rclcpp::ParameterValue( 1.5 ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".rotate_to_heading_angular_vel", rclcpp::ParameterValue( 1.8 ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue( 0.1 ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".use_velocity_scaled_lookahead_dist",
rclcpp::ParameterValue( false ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".min_approach_linear_velocity", rclcpp::ParameterValue( 0.05 ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot",
rclcpp::ParameterValue( 1.0 ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue( true ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".use_cost_regulated_linear_velocity_scaling",
rclcpp::ParameterValue( true ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".cost_scaling_dist", rclcpp::ParameterValue( 0.6 ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".cost_scaling_gain", rclcpp::ParameterValue( 1.0 ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".inflation_cost_scaling_factor", rclcpp::ParameterValue( 3.0 ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".regulated_linear_scaling_min_radius", rclcpp::ParameterValue( 0.90 ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".regulated_linear_scaling_min_speed", rclcpp::ParameterValue( 0.25 ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".use_rotate_to_heading", rclcpp::ParameterValue( true ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue( 0.785 ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue( 3.2 ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".allow_reversing", rclcpp::ParameterValue( false ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".max_robot_pose_search_dist",
rclcpp::ParameterValue( getCostmapMaxExtent( ) ) );
declare_parameter_if_not_declared(
node, plugin_name_ + ".use_interpolation",
rclcpp::ParameterValue( true ) );
node->get_parameter( plugin_name_ + ".desired_linear_vel", desired_linear_vel_ );
base_desired_linear_vel_ = desired_linear_vel_;
node->get_parameter( plugin_name_ + ".lookahead_dist", lookahead_dist_ );
node->get_parameter( plugin_name_ + ".min_lookahead_dist", min_lookahead_dist_ );
node->get_parameter( plugin_name_ + ".max_lookahead_dist", max_lookahead_dist_ );
node->get_parameter( plugin_name_ + ".lookahead_time", lookahead_time_ );
node->get_parameter(
plugin_name_ + ".rotate_to_heading_angular_vel",
rotate_to_heading_angular_vel_ );
node->get_parameter( plugin_name_ + ".transform_tolerance", transform_tolerance );
node->get_parameter(
plugin_name_ + ".use_velocity_scaled_lookahead_dist",
use_velocity_scaled_lookahead_dist_ );
node->get_parameter(
plugin_name_ + ".min_approach_linear_velocity",
min_approach_linear_velocity_ );
node->get_parameter(
plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot",
max_allowed_time_to_collision_up_to_carrot_ );
node->get_parameter(
plugin_name_ + ".use_regulated_linear_velocity_scaling",
use_regulated_linear_velocity_scaling_ );
node->get_parameter(
plugin_name_ + ".use_cost_regulated_linear_velocity_scaling",
use_cost_regulated_linear_velocity_scaling_ );
node->get_parameter( plugin_name_ + ".cost_scaling_dist", cost_scaling_dist_ );
node->get_parameter( plugin_name_ + ".cost_scaling_gain", cost_scaling_gain_ );
node->get_parameter(
plugin_name_ + ".inflation_cost_scaling_factor",
inflation_cost_scaling_factor_ );
node->get_parameter(
plugin_name_ + ".regulated_linear_scaling_min_radius",
regulated_linear_scaling_min_radius_ );
node->get_parameter(
plugin_name_ + ".regulated_linear_scaling_min_speed",
regulated_linear_scaling_min_speed_ );
node->get_parameter( plugin_name_ + ".use_rotate_to_heading", use_rotate_to_heading_ );
node->get_parameter( plugin_name_ + ".rotate_to_heading_min_angle", rotate_to_heading_min_angle_ );
node->get_parameter( plugin_name_ + ".max_angular_accel", max_angular_accel_ );
node->get_parameter( plugin_name_ + ".allow_reversing", allow_reversing_ );
node->get_parameter( "controller_frequency", control_frequency );
node->get_parameter(
plugin_name_ + ".max_robot_pose_search_dist",
max_robot_pose_search_dist_ );
node->get_parameter(
plugin_name_ + ".use_interpolation",
use_interpolation_ );
transform_tolerance_ = tf2::durationFromSec( transform_tolerance );
control_duration_ = 1.0 / control_frequency;
if ( inflation_cost_scaling_factor_ <= 0.0 ) {
RCLCPP_WARN(
logger_, "The value inflation_cost_scaling_factor is incorrectly set, "
"it should be >0. Disabling cost regulated linear velocity scaling." );
use_cost_regulated_linear_velocity_scaling_ = false;
}
/** Possible to drive in reverse direction if and only if
"use_rotate_to_heading" parameter is set to false **/
if ( use_rotate_to_heading_ && allow_reversing_ ) {
RCLCPP_WARN(
logger_, "Disabling reversing. Both use_rotate_to_heading and allow_reversing "
"parameter cannot be set to true. By default setting use_rotate_to_heading true" );
allow_reversing_ = false;
}
global_path_pub_ = node->create_publisher<nav_msgs::msg::Path>( "received_global_plan", 1 );
carrot_pub_ = node->create_publisher<geometry_msgs::msg::PointStamped>( "lookahead_point", 1 );
carrot_arc_pub_ = node->create_publisher<nav_msgs::msg::Path>( "lookahead_collision_arc", 1 );
// initialize collision checker and set costmap
collision_checker_ = std::make_unique<nav2_costmap_2d::
FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>( costmap_ );
collision_checker_->setCostmap( costmap_ );
}
193 void RegulatedPurePursuitController::cleanup( )
{
RCLCPP_INFO(
logger_,
"Cleaning up controller: %s of type"
" regulated_pure_pursuit_controller::RegulatedPurePursuitController",
plugin_name_.c_str( ) );
global_path_pub_.reset( );
carrot_pub_.reset( );
carrot_arc_pub_.reset( );
}
205 void RegulatedPurePursuitController::activate( )
{
RCLCPP_INFO(
logger_,
"Activating controller: %s of type "
"regulated_pure_pursuit_controller::RegulatedPurePursuitController",
plugin_name_.c_str( ) );
global_path_pub_->on_activate( );
carrot_pub_->on_activate( );
carrot_arc_pub_->on_activate( );
// Add callback for dynamic parameters
auto node = node_.lock( );
dyn_params_handler_ = node->add_on_set_parameters_callback(
std::bind(
&RegulatedPurePursuitController::dynamicParametersCallback,
this, std::placeholders::_1 ) );
}
223 void RegulatedPurePursuitController::deactivate( )
{
RCLCPP_INFO(
logger_,
"Deactivating controller: %s of type "
"regulated_pure_pursuit_controller::RegulatedPurePursuitController",
plugin_name_.c_str( ) );
global_path_pub_->on_deactivate( );
carrot_pub_->on_deactivate( );
carrot_arc_pub_->on_deactivate( );
dyn_params_handler_.reset( );
}
236 std::unique_ptr<geometry_msgs::msg::PointStamped> RegulatedPurePursuitController::createCarrotMsg(
237 const geometry_msgs::msg::PoseStamped & carrot_pose )
{
auto carrot_msg = std::make_unique<geometry_msgs::msg::PointStamped>( );
carrot_msg->header = carrot_pose.header;
carrot_msg->point.x = carrot_pose.pose.position.x;
carrot_msg->point.y = carrot_pose.pose.position.y;
carrot_msg->point.z = 0.01; // publish right over map to stand out
return carrot_msg;
}
247 double RegulatedPurePursuitController::getLookAheadDistance(
248 const geometry_msgs::msg::Twist & speed )
{
// If using velocity-scaled look ahead distances, find and clamp the dist
// Else, use the static look ahead distance
double lookahead_dist = lookahead_dist_;
if ( use_velocity_scaled_lookahead_dist_ ) {
lookahead_dist = fabs( speed.linear.x ) * lookahead_time_;
lookahead_dist = std::clamp( lookahead_dist, min_lookahead_dist_, max_lookahead_dist_ );
}
return lookahead_dist;
}
261 geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocityCommands(
262 const geometry_msgs::msg::PoseStamped & pose,
263 const geometry_msgs::msg::Twist & speed,
264 nav2_core::GoalChecker * goal_checker )
{
std::lock_guard<std::mutex> lock_reinit( mutex_ );
// Update for the current goal checker's state
geometry_msgs::msg::Pose pose_tolerance;
geometry_msgs::msg::Twist vel_tolerance;
if ( !goal_checker->getTolerances( pose_tolerance, vel_tolerance ) ) {
RCLCPP_WARN( logger_, "Unable to retrieve goal checker's tolerances!" );
} else {
goal_dist_tol_ = pose_tolerance.position.x;
}
// Transform path to robot base frame
auto transformed_plan = transformGlobalPlan( pose );
// Find look ahead distance and point on path and publish
double lookahead_dist = getLookAheadDistance( speed );
// Check for reverse driving
if ( allow_reversing_ ) {
// Cusp check
double dist_to_cusp = findVelocitySignChange( transformed_plan );
// if the lookahead distance is further than the cusp, use the cusp distance instead
if ( dist_to_cusp < lookahead_dist ) {
lookahead_dist = dist_to_cusp;
}
}
auto carrot_pose = getLookAheadPoint( lookahead_dist, transformed_plan );
carrot_pub_->publish( createCarrotMsg( carrot_pose ) );
double linear_vel, angular_vel;
// Find distance^2 to look ahead point ( carrot ) in robot base frame
// This is the chord length of the circle
const double carrot_dist2 =
( carrot_pose.pose.position.x * carrot_pose.pose.position.x ) +
( carrot_pose.pose.position.y * carrot_pose.pose.position.y );
// Find curvature of circle ( k = 1 / R )
double curvature = 0.0;
if ( carrot_dist2 > 0.001 ) {
curvature = 2.0 * carrot_pose.pose.position.y / carrot_dist2;
}
// Setting the velocity direction
double sign = 1.0;
if ( allow_reversing_ ) {
sign = carrot_pose.pose.position.x >= 0.0 ? 1.0 : -1.0;
}
linear_vel = desired_linear_vel_;
// Make sure we're in compliance with basic constraints
double angle_to_heading;
if ( shouldRotateToGoalHeading( carrot_pose ) ) {
double angle_to_goal = tf2::getYaw( transformed_plan.poses.back( ).pose.orientation );
rotateToHeading( linear_vel, angular_vel, angle_to_goal, speed );
} else if ( shouldRotateToPath( carrot_pose, angle_to_heading ) ) {
rotateToHeading( linear_vel, angular_vel, angle_to_heading, speed );
} else {
applyConstraints(
fabs( lookahead_dist - sqrt( carrot_dist2 ) ),
lookahead_dist, curvature, speed,
costAtPose( pose.pose.position.x, pose.pose.position.y ), linear_vel, sign );
// Apply curvature to angular velocity after constraining linear velocity
angular_vel = sign * linear_vel * curvature;
}
// Collision checking on this velocity heading
const double & carrot_dist = hypot( carrot_pose.pose.position.x, carrot_pose.pose.position.y );
if ( isCollisionImminent( pose, linear_vel, angular_vel, carrot_dist ) ) {
throw nav2_core::PlannerException( "RegulatedPurePursuitController detected collision ahead!" );
}
// populate and return message
geometry_msgs::msg::TwistStamped cmd_vel;
cmd_vel.header = pose.header;
cmd_vel.twist.linear.x = linear_vel;
cmd_vel.twist.angular.z = angular_vel;
return cmd_vel;
}
350 bool RegulatedPurePursuitController::shouldRotateToPath(
351 const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path )
{
// Whether we should rotate robot to rough path heading
angle_to_path = atan2( carrot_pose.pose.position.y, carrot_pose.pose.position.x );
return use_rotate_to_heading_ && fabs( angle_to_path ) > rotate_to_heading_min_angle_;
}
358 bool RegulatedPurePursuitController::shouldRotateToGoalHeading(
359 const geometry_msgs::msg::PoseStamped & carrot_pose )
{
// Whether we should rotate robot to goal heading
double dist_to_goal = std::hypot( carrot_pose.pose.position.x, carrot_pose.pose.position.y );
return use_rotate_to_heading_ && dist_to_goal < goal_dist_tol_;
}
366 void RegulatedPurePursuitController::rotateToHeading(
double & linear_vel, double & angular_vel,
368 const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed )
{
// Rotate in place using max angular velocity / acceleration possible
linear_vel = 0.0;
const double sign = angle_to_path > 0.0 ? 1.0 : -1.0;
angular_vel = sign * rotate_to_heading_angular_vel_;
const double & dt = control_duration_;
const double min_feasible_angular_speed = curr_speed.angular.z - max_angular_accel_ * dt;
const double max_feasible_angular_speed = curr_speed.angular.z + max_angular_accel_ * dt;
angular_vel = std::clamp( angular_vel, min_feasible_angular_speed, max_feasible_angular_speed );
}
381 geometry_msgs::msg::Point RegulatedPurePursuitController::circleSegmentIntersection(
382 const geometry_msgs::msg::Point & p1,
383 const geometry_msgs::msg::Point & p2,
double r )
{
// Formula for intersection of a line with a circle centered at the origin,
// modified to always return the point that is on the segment between the two points.
// https://mathworld.wolfram.com/Circle-LineIntersection.html
// This works because the poses are transformed into the robot frame.
// This can be derived from solving the system of equations of a line and a circle
// which results in something that is just a reformulation of the quadratic formula.
// Interactive illustration in doc/circle-segment-intersection.ipynb as well as at
// https://www.desmos.com/calculator/td5cwbuocd
double x1 = p1.x;
double x2 = p2.x;
double y1 = p1.y;
double y2 = p2.y;
double dx = x2 - x1;
double dy = y2 - y1;
double dr2 = dx * dx + dy * dy;
double D = x1 * y2 - x2 * y1;
// Augmentation to only return point within segment
double d1 = x1 * x1 + y1 * y1;
double d2 = x2 * x2 + y2 * y2;
double dd = d2 - d1;
geometry_msgs::msg::Point p;
double sqrt_term = std::sqrt( r * r * dr2 - D * D );
p.x = ( D * dy + std::copysign( 1.0, dd ) * dx * sqrt_term ) / dr2;
p.y = ( -D * dx + std::copysign( 1.0, dd ) * dy * sqrt_term ) / dr2;
return p;
}
416 geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint(
const double & lookahead_dist,
418 const nav_msgs::msg::Path & transformed_plan )
{
// Find the first pose which is at a distance greater than the lookahead distance
auto goal_pose_it = std::find_if(
transformed_plan.poses.begin( ), transformed_plan.poses.end( ), [&]( const auto & ps ) {
return hypot( ps.pose.position.x, ps.pose.position.y ) >= lookahead_dist;
} );
// If the no pose is not far enough, take the last pose
if ( goal_pose_it == transformed_plan.poses.end( ) ) {
goal_pose_it = std::prev( transformed_plan.poses.end( ) );
} else if ( use_interpolation_ && goal_pose_it != transformed_plan.poses.begin( ) ) {
// Find the point on the line segment between the two poses
// that is exactly the lookahead distance away from the robot pose ( the origin )
// This can be found with a closed form for the intersection of a segment and a circle
// Because of the way we did the std::find_if, prev_pose is guaranteed to be inside the circle,
// and goal_pose is guaranteed to be outside the circle.
auto prev_pose_it = std::prev( goal_pose_it );
auto point = circleSegmentIntersection(
prev_pose_it->pose.position,
goal_pose_it->pose.position, lookahead_dist );
geometry_msgs::msg::PoseStamped pose;
pose.header.frame_id = prev_pose_it->header.frame_id;
pose.header.stamp = goal_pose_it->header.stamp;
pose.pose.position = point;
return pose;
}
return *goal_pose_it;
}
449 bool RegulatedPurePursuitController::isCollisionImminent(
450 const geometry_msgs::msg::PoseStamped & robot_pose,
const double & linear_vel, const double & angular_vel,
const double & carrot_dist )
{
// Note( stevemacenski ): This may be a bit unusual, but the robot_pose is in
// odom frame and the carrot_pose is in robot base frame.
// check current point is OK
if ( inCollision(
robot_pose.pose.position.x, robot_pose.pose.position.y,
tf2::getYaw( robot_pose.pose.orientation ) ) )
{
return true;
}
// visualization messages
nav_msgs::msg::Path arc_pts_msg;
arc_pts_msg.header.frame_id = costmap_ros_->getGlobalFrameID( );
arc_pts_msg.header.stamp = robot_pose.header.stamp;
geometry_msgs::msg::PoseStamped pose_msg;
pose_msg.header.frame_id = arc_pts_msg.header.frame_id;
pose_msg.header.stamp = arc_pts_msg.header.stamp;
double projection_time = 0.0;
if ( fabs( linear_vel ) < 0.01 && fabs( angular_vel ) > 0.01 ) {
// rotating to heading at goal or toward path
// Equation finds the angular distance required for the largest
// part of the robot radius to move to another costmap cell:
// theta_min = 2.0 * sin ( ( res/2 ) / r_max )
// via isosceles triangle r_max-r_max-resolution,
// dividing by angular_velocity gives us a timestep.
double max_radius = costmap_ros_->getLayeredCostmap( )->getCircumscribedRadius( );
projection_time =
2.0 * sin( ( costmap_->getResolution( ) / 2 ) / max_radius ) / fabs( angular_vel );
} else {
// Normal path tracking
projection_time = costmap_->getResolution( ) / fabs( linear_vel );
}
const geometry_msgs::msg::Point & robot_xy = robot_pose.pose.position;
geometry_msgs::msg::Pose2D curr_pose;
curr_pose.x = robot_pose.pose.position.x;
curr_pose.y = robot_pose.pose.position.y;
curr_pose.theta = tf2::getYaw( robot_pose.pose.orientation );
// only forward simulate within time requested
int i = 1;
while ( i * projection_time < max_allowed_time_to_collision_up_to_carrot_ ) {
i++;
// apply velocity at curr_pose over distance
curr_pose.x += projection_time * ( linear_vel * cos( curr_pose.theta ) );
curr_pose.y += projection_time * ( linear_vel * sin( curr_pose.theta ) );
curr_pose.theta += projection_time * angular_vel;
// check if past carrot pose, where no longer a thoughtfully valid command
if ( hypot( curr_pose.x - robot_xy.x, curr_pose.y - robot_xy.y ) > carrot_dist ) {
break;
}
// store it for visualization
pose_msg.pose.position.x = curr_pose.x;
pose_msg.pose.position.y = curr_pose.y;
pose_msg.pose.position.z = 0.01;
arc_pts_msg.poses.push_back( pose_msg );
// check for collision at the projected pose
if ( inCollision( curr_pose.x, curr_pose.y, curr_pose.theta ) ) {
carrot_arc_pub_->publish( arc_pts_msg );
return true;
}
}
carrot_arc_pub_->publish( arc_pts_msg );
return false;
}
528 bool RegulatedPurePursuitController::inCollision(
const double & x,
const double & y,
const double & theta )
{
unsigned int mx, my;
if ( !costmap_->worldToMap( x, y, mx, my ) ) {
RCLCPP_WARN_THROTTLE(
logger_, *( clock_ ), 30000,
"The dimensions of the costmap is too small to successfully check for "
"collisions as far ahead as requested. Proceed at your own risk, slow the robot, or "
"increase your costmap size." );
return false;
}
double footprint_cost = collision_checker_->footprintCostAtPose(
x, y, theta, costmap_ros_->getRobotFootprint( ) );
if ( footprint_cost == static_cast<double>( NO_INFORMATION ) &&
costmap_ros_->getLayeredCostmap( )->isTrackingUnknown( ) )
{
return false;
}
// if occupied or unknown and not to traverse unknown space
return footprint_cost >= static_cast<double>( LETHAL_OBSTACLE );
}
556 double RegulatedPurePursuitController::costAtPose( const double & x, const double & y )
{
unsigned int mx, my;
if ( !costmap_->worldToMap( x, y, mx, my ) ) {
RCLCPP_FATAL(
logger_,
"The dimensions of the costmap is too small to fully include your robot's footprint, "
"thusly the robot cannot proceed further" );
throw nav2_core::PlannerException(
"RegulatedPurePursuitController: Dimensions of the costmap are too small "
"to encapsulate the robot footprint at current speeds!" );
}
unsigned char cost = costmap_->getCost( mx, my );
return static_cast<double>( cost );
}
574 void RegulatedPurePursuitController::applyConstraints(
const double & dist_error, const double & lookahead_dist,
576 const double & curvature, const geometry_msgs::msg::Twist & /*curr_speed*/,
const double & pose_cost, double & linear_vel, double & sign )
{
double curvature_vel = linear_vel;
double cost_vel = linear_vel;
double approach_vel = linear_vel;
// limit the linear velocity by curvature
const double radius = fabs( 1.0 / curvature );
const double & min_rad = regulated_linear_scaling_min_radius_;
if ( use_regulated_linear_velocity_scaling_ && radius < min_rad ) {
curvature_vel *= 1.0 - ( fabs( radius - min_rad ) / min_rad );
}
// limit the linear velocity by proximity to obstacles
if ( use_cost_regulated_linear_velocity_scaling_ &&
pose_cost != static_cast<double>( NO_INFORMATION ) &&
pose_cost != static_cast<double>( FREE_SPACE ) )
{
const double inscribed_radius = costmap_ros_->getLayeredCostmap( )->getInscribedRadius( );
const double min_distance_to_obstacle = ( -1.0 / inflation_cost_scaling_factor_ ) *
std::log( pose_cost / ( INSCRIBED_INFLATED_OBSTACLE - 1 ) ) + inscribed_radius;
if ( min_distance_to_obstacle < cost_scaling_dist_ ) {
cost_vel *= cost_scaling_gain_ * min_distance_to_obstacle / cost_scaling_dist_;
}
}
// Use the lowest of the 2 constraint heuristics, but above the minimum translational speed
linear_vel = std::min( cost_vel, curvature_vel );
linear_vel = std::max( linear_vel, regulated_linear_scaling_min_speed_ );
// if the actual lookahead distance is shorter than requested, that means we're at the
// end of the path. We'll scale linear velocity by error to slow to a smooth stop.
// This expression is eq. to ( 1 ) holding time to goal, t, constant using the theoretical
// lookahead distance and proposed velocity and ( 2 ) using t with the actual lookahead
// distance to scale the velocity ( e.g. t = lookahead / velocity, v = carrot / t ).
if ( dist_error > 2.0 * costmap_->getResolution( ) ) {
double velocity_scaling = 1.0 - ( dist_error / lookahead_dist );
double unbounded_vel = approach_vel * velocity_scaling;
if ( unbounded_vel < min_approach_linear_velocity_ ) {
approach_vel = min_approach_linear_velocity_;
} else {
approach_vel *= velocity_scaling;
}
// Use the lowest velocity between approach and other constraints, if all overlapping
linear_vel = std::min( linear_vel, approach_vel );
}
// Limit linear velocities to be valid
linear_vel = std::clamp( fabs( linear_vel ), 0.0, desired_linear_vel_ );
linear_vel = sign * linear_vel;
}
631 void RegulatedPurePursuitController::setPlan( const nav_msgs::msg::Path & path )
{
global_plan_ = path;
}
636 void RegulatedPurePursuitController::setSpeedLimit(
const double & speed_limit,
638 const bool & percentage )
{
if ( speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT ) {
// Restore default value
desired_linear_vel_ = base_desired_linear_vel_;
} else {
if ( percentage ) {
// Speed limit is expressed in % from maximum speed of robot
desired_linear_vel_ = base_desired_linear_vel_ * speed_limit / 100.0;
} else {
// Speed limit is expressed in absolute value
desired_linear_vel_ = speed_limit;
}
}
}
654 nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan(
655 const geometry_msgs::msg::PoseStamped & pose )
{
if ( global_plan_.poses.empty( ) ) {
throw nav2_core::PlannerException( "Received plan with zero length" );
}
// let's get the pose of the robot in the frame of the plan
geometry_msgs::msg::PoseStamped robot_pose;
if ( !transformPose( global_plan_.header.frame_id, pose, robot_pose ) ) {
throw nav2_core::PlannerException( "Unable to transform robot pose into global plan's frame" );
}
// We'll discard points on the plan that are outside the local costmap
double max_costmap_extent = getCostmapMaxExtent( );
auto closest_pose_upper_bound =
nav2_util::geometry_utils::first_after_integrated_distance(
global_plan_.poses.begin( ), global_plan_.poses.end( ), max_robot_pose_search_dist_ );
// First find the closest pose on the path to the robot
// bounded by when the path turns around ( if it does ) so we don't get a pose from a later
// portion of the path
auto transformation_begin =
nav2_util::geometry_utils::min_by(
global_plan_.poses.begin( ), closest_pose_upper_bound,
[&robot_pose]( const geometry_msgs::msg::PoseStamped & ps ) {
return euclidean_distance( robot_pose, ps );
} );
// Find points up to max_transform_dist so we only transform them.
auto transformation_end = std::find_if(
transformation_begin, global_plan_.poses.end( ),
[&]( const auto & pose ) {
return euclidean_distance( pose, robot_pose ) > max_costmap_extent;
} );
// Lambda to transform a PoseStamped from global frame to local
auto transformGlobalPoseToLocal = [&]( const auto & global_plan_pose ) {
geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose;
stamped_pose.header.frame_id = global_plan_.header.frame_id;
stamped_pose.header.stamp = robot_pose.header.stamp;
stamped_pose.pose = global_plan_pose.pose;
transformPose( costmap_ros_->getBaseFrameID( ), stamped_pose, transformed_pose );
transformed_pose.pose.position.z = 0.0;
return transformed_pose;
};
// Transform the near part of the global plan into the robot's frame of reference.
nav_msgs::msg::Path transformed_plan;
std::transform(
transformation_begin, transformation_end,
std::back_inserter( transformed_plan.poses ),
transformGlobalPoseToLocal );
transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID( );
transformed_plan.header.stamp = robot_pose.header.stamp;
// Remove the portion of the global plan that we've already passed so we don't
// process it on the next iteration ( this is called path pruning )
global_plan_.poses.erase( begin( global_plan_.poses ), transformation_begin );
global_path_pub_->publish( transformed_plan );
if ( transformed_plan.poses.empty( ) ) {
throw nav2_core::PlannerException( "Resulting plan has 0 poses in it." );
}
return transformed_plan;
}
723 double RegulatedPurePursuitController::findVelocitySignChange(
724 const nav_msgs::msg::Path & transformed_plan )
{
// Iterating through the transformed global path to determine the position of the cusp
for ( unsigned int pose_id = 1; pose_id < transformed_plan.poses.size( ) - 1; ++pose_id ) {
// We have two vectors for the dot product OA and AB. Determining the vectors.
double oa_x = transformed_plan.poses[pose_id].pose.position.x -
transformed_plan.poses[pose_id - 1].pose.position.x;
double oa_y = transformed_plan.poses[pose_id].pose.position.y -
transformed_plan.poses[pose_id - 1].pose.position.y;
double ab_x = transformed_plan.poses[pose_id + 1].pose.position.x -
transformed_plan.poses[pose_id].pose.position.x;
double ab_y = transformed_plan.poses[pose_id + 1].pose.position.y -
transformed_plan.poses[pose_id].pose.position.y;
/* Checking for the existance of cusp, in the path, using the dot product
and determine it's distance from the robot. If there is no cusp in the path,
then just determine the distance to the goal location. */
if ( ( oa_x * ab_x ) + ( oa_y * ab_y ) < 0.0 ) {
// returning the distance if there is a cusp
// The transformed path is in the robots frame, so robot is at the origin
return hypot(
transformed_plan.poses[pose_id].pose.position.x,
transformed_plan.poses[pose_id].pose.position.y );
}
}
return std::numeric_limits<double>::max( );
}
753 bool RegulatedPurePursuitController::transformPose(
754 const std::string frame,
755 const geometry_msgs::msg::PoseStamped & in_pose,
756 geometry_msgs::msg::PoseStamped & out_pose ) const
{
if ( in_pose.header.frame_id == frame ) {
out_pose = in_pose;
return true;
}
try {
tf_->transform( in_pose, out_pose, frame, transform_tolerance_ );
out_pose.header.frame_id = frame;
return true;
} catch ( tf2::TransformException & ex ) {
RCLCPP_ERROR( logger_, "Exception in transformPose: %s", ex.what( ) );
}
return false;
}
773 double RegulatedPurePursuitController::getCostmapMaxExtent( ) const
{
const double max_costmap_dim_meters = std::max(
costmap_->getSizeInMetersX( ), costmap_->getSizeInMetersX( ) );
return max_costmap_dim_meters / 2.0;
}
rcl_interfaces::msg::SetParametersResult
782 RegulatedPurePursuitController::dynamicParametersCallback(
783 std::vector<rclcpp::Parameter> parameters )
{
rcl_interfaces::msg::SetParametersResult result;
std::lock_guard<std::mutex> lock_reinit( mutex_ );
for ( auto parameter : parameters ) {
const auto & type = parameter.get_type( );
const auto & name = parameter.get_name( );
if ( type == ParameterType::PARAMETER_DOUBLE ) {
if ( name == plugin_name_ + ".inflation_cost_scaling_factor" ) {
if ( parameter.as_double( ) <= 0.0 ) {
RCLCPP_WARN(
logger_, "The value inflation_cost_scaling_factor is incorrectly set, "
"it should be >0. Ignoring parameter update." );
continue;
}
inflation_cost_scaling_factor_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".desired_linear_vel" ) {
desired_linear_vel_ = parameter.as_double( );
base_desired_linear_vel_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".lookahead_dist" ) {
lookahead_dist_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".max_lookahead_dist" ) {
max_lookahead_dist_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".min_lookahead_dist" ) {
min_lookahead_dist_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".lookahead_time" ) {
lookahead_time_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".rotate_to_heading_angular_vel" ) {
rotate_to_heading_angular_vel_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".min_approach_linear_velocity" ) {
min_approach_linear_velocity_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot" ) {
max_allowed_time_to_collision_up_to_carrot_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".cost_scaling_dist" ) {
cost_scaling_dist_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".cost_scaling_gain" ) {
cost_scaling_gain_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".regulated_linear_scaling_min_radius" ) {
regulated_linear_scaling_min_radius_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".transform_tolerance" ) {
double transform_tolerance = parameter.as_double( );
transform_tolerance_ = tf2::durationFromSec( transform_tolerance );
} else if ( name == plugin_name_ + ".regulated_linear_scaling_min_speed" ) {
regulated_linear_scaling_min_speed_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".max_angular_accel" ) {
max_angular_accel_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".rotate_to_heading_min_angle" ) {
rotate_to_heading_min_angle_ = parameter.as_double( );
}
} else if ( type == ParameterType::PARAMETER_BOOL ) {
if ( name == plugin_name_ + ".use_velocity_scaled_lookahead_dist" ) {
use_velocity_scaled_lookahead_dist_ = parameter.as_bool( );
} else if ( name == plugin_name_ + ".use_regulated_linear_velocity_scaling" ) {
use_regulated_linear_velocity_scaling_ = parameter.as_bool( );
} else if ( name == plugin_name_ + ".use_cost_regulated_linear_velocity_scaling" ) {
use_cost_regulated_linear_velocity_scaling_ = parameter.as_bool( );
} else if ( name == plugin_name_ + ".use_rotate_to_heading" ) {
if ( parameter.as_bool( ) && allow_reversing_ ) {
RCLCPP_WARN(
logger_, "Both use_rotate_to_heading and allow_reversing "
"parameter cannot be set to true. Rejecting parameter update." );
continue;
}
use_rotate_to_heading_ = parameter.as_bool( );
} else if ( name == plugin_name_ + ".allow_reversing" ) {
if ( use_rotate_to_heading_ && parameter.as_bool( ) ) {
RCLCPP_WARN(
logger_, "Both use_rotate_to_heading and allow_reversing "
"parameter cannot be set to true. Rejecting parameter update." );
continue;
}
allow_reversing_ = parameter.as_bool( );
}
}
}
result.successful = true;
return result;
}
} // namespace nav2_regulated_pure_pursuit_controller
// Register this controller as a nav2_core plugin
868 PLUGINLIB_EXPORT_CLASS(
nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController,
nav2_core::Controller )
1 // Copyright ( c ) 2022 Adam Aposhian
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include "path_utils.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
namespace path_utils
{
24 void append_transform_to_path(
nav_msgs::msg::Path & path,
tf2::Transform & relative_transform )
{
// Add a new empty pose
path.poses.emplace_back( );
// Get the previous, last pose ( after the emplace_back so the reference isn't invalidated )
auto & previous_pose = *( path.poses.end( ) - 2 );
auto & new_pose = path.poses.back( );
// get map_transform of previous_pose
tf2::Transform map_transform;
tf2::fromMsg( previous_pose.pose, map_transform );
tf2::Transform full_transform;
full_transform.mult( map_transform, relative_transform );
tf2::toMsg( full_transform, new_pose.pose );
new_pose.header.frame_id = previous_pose.header.frame_id;
}
45 void Straight::append( nav_msgs::msg::Path & path, double spacing ) const
{
auto num_points = std::floor( length_ / spacing );
path.poses.reserve( path.poses.size( ) + num_points );
tf2::Transform translation( tf2::Quaternion::getIdentity( ), tf2::Vector3( spacing, 0.0, 0.0 ) );
for ( size_t i = 1; i <= num_points; ++i ) {
append_transform_to_path( path, translation );
}
}
55 double chord_length( double radius, double radians )
{
return 2 * radius * sin( radians / 2 );
}
60 void Arc::append( nav_msgs::msg::Path & path, double spacing ) const
{
double length = radius_ * std::abs( radians_ );
size_t num_points = std::floor( length / spacing );
double radians_per_step = radians_ / num_points;
tf2::Transform transform(
tf2::Quaternion( tf2::Vector3( 0.0, 0.0, 1.0 ), radians_per_step ),
tf2::Vector3( chord_length( radius_, std::abs( radians_per_step ) ), 0.0, 0.0 ) );
path.poses.reserve( path.poses.size( ) + num_points );
for ( size_t i = 0; i < num_points; ++i ) {
append_transform_to_path( path, transform );
}
}
74 nav_msgs::msg::Path generate_path(
geometry_msgs::msg::PoseStamped start,
double spacing,
std::initializer_list<std::unique_ptr<PathSegment>> segments )
{
nav_msgs::msg::Path path;
path.header = start.header;
path.poses.push_back( start );
for ( const auto & segment : segments ) {
segment->append( path, spacing );
}
return path;
}
} // namespace path_utils
1 // Copyright ( c ) 2022 FireFly Automatix
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Author: Adam Aposhian
#ifndef PATH_UTILS__PATH_UTILS_HPP_
#define PATH_UTILS__PATH_UTILS_HPP_
#include <cmath>
#include <initializer_list>
#include <memory>
#include "nav_msgs/msg/path.hpp"
namespace path_utils
{
/**
* Build human-readable test paths
*/
class PathSegment
{
public:
virtual void append( nav_msgs::msg::Path & path, double spacing ) const = 0;
virtual ~PathSegment( ) {}
};
39 class Arc : public PathSegment
{
public:
42 explicit Arc( double radius, double radians )
: radius_( radius ), radians_( radians ) {}
void append( nav_msgs::msg::Path & path, double spacing ) const override;
private:
double radius_;
double radians_;
};
51 class Straight : public PathSegment
{
public:
54 explicit Straight( double length )
: length_( length ) {}
void append( nav_msgs::msg::Path & path, double spacing ) const override;
private:
double length_;
};
62 class LeftTurn : public Arc
{
public:
65 explicit LeftTurn( double radius )
: Arc( radius, M_PI_2 ) {}
};
69 class RightTurn : public Arc
{
public:
explicit RightTurn( double radius )
: Arc( radius, -M_PI_2 ) {}
};
76 class LeftTurnAround : public Arc
{
public:
79 explicit LeftTurnAround( double radius )
: Arc( radius, M_PI ) {}
};
83 class RightTurnAround : public Arc
{
public:
explicit RightTurnAround( double radius )
: Arc( radius, -M_PI ) {}
};
90 class LeftCircle : public Arc
{
public:
93 explicit LeftCircle( double radius )
: Arc( radius, 2.0 * M_PI ) {}
};
97 class RightCircle : public Arc
{
public:
100 explicit RightCircle( double radius )
: Arc( radius, -2.0 * M_PI ) {}
};
104 nav_msgs::msg::Path generate_path(
geometry_msgs::msg::PoseStamped start,
double spacing,
std::initializer_list<std::unique_ptr<PathSegment>> segments );
} // namespace path_utils
#endif // PATH_UTILS__PATH_UTILS_HPP_
1 // Copyright ( c ) 2022 Adam Aposhian
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include "path_utils.hpp"
#include "gtest/gtest.h"
using namespace path_utils; // NOLINT
22 TEST( PathUtils, test_generate_straight )
{
geometry_msgs::msg::PoseStamped start;
start.header.frame_id = "test_frame";
constexpr double path_length = 2.0;
constexpr double spacing = 1.0;
auto path = generate_path(
start, spacing, {
std::make_unique<Straight>( path_length )
} );
EXPECT_EQ( path.poses.size( ), 3u );
for ( const auto & pose : path.poses ) {
EXPECT_EQ( pose.header.frame_id, start.header.frame_id );
}
EXPECT_DOUBLE_EQ( path.poses[0].pose.position.x, 0.0 );
EXPECT_DOUBLE_EQ( path.poses[0].pose.position.y, 0.0 );
EXPECT_DOUBLE_EQ( path.poses[0].pose.position.z, 0.0 );
EXPECT_NEAR( path.poses[1].pose.position.x, 1.0, 0.1 );
EXPECT_NEAR( path.poses[1].pose.position.y, 0.0, 0.1 );
EXPECT_NEAR( path.poses[1].pose.position.z, 0.0, 0.1 );
EXPECT_NEAR( path.poses[2].pose.position.x, 2.0, 0.1 );
EXPECT_NEAR( path.poses[2].pose.position.y, 0.0, 0.1 );
EXPECT_NEAR( path.poses[2].pose.position.z, 0.0, 0.1 );
}
51 TEST( PathUtils, test_half_turn )
{
// Start at a more interesting place, turned the other way
geometry_msgs::msg::PoseStamped start;
start.header.frame_id = "map";
start.pose.position.x = 1.0;
start.pose.position.y = -1.0;
start.pose.orientation.x = 0.0;
start.pose.orientation.y = 0.0;
start.pose.orientation.z = 1.0;
start.pose.orientation.w = 0.0;
constexpr double spacing = 0.1;
constexpr double radius = 2.0;
auto path = generate_path(
start, spacing, {
std::make_unique<RightTurnAround>( radius ),
} );
constexpr double expected_path_length = M_PI * radius;
EXPECT_NEAR( path.poses.size( ), 1 + static_cast<std::size_t>( expected_path_length / spacing ), 10 );
for ( const auto & pose : path.poses ) {
EXPECT_EQ( pose.header.frame_id, start.header.frame_id );
}
// Check the last pose
auto & last_pose = path.poses.back( );
auto & last_position = last_pose.pose.position;
EXPECT_NEAR( last_position.x, 1.0, 0.2 );
EXPECT_NEAR( last_position.y, 3.0, 0.2 );
EXPECT_DOUBLE_EQ( last_position.z, 0.0 );
// Should be facing forward now
auto & last_orientation = last_pose.pose.orientation;
EXPECT_NEAR( last_orientation.x, 0.0, 0.1 );
EXPECT_NEAR( last_orientation.y, 0.0, 0.1 );
EXPECT_NEAR( last_orientation.z, 0.0, 0.1 );
EXPECT_NEAR( last_orientation.w, 1.0, 0.1 );
}
91 TEST( PathUtils, test_generate_all )
{
geometry_msgs::msg::PoseStamped start;
start.header.frame_id = "map";
constexpr double spacing = 0.1;
auto path = generate_path(
start, spacing, {
std::make_unique<Straight>( 1.0 ),
std::make_unique<LeftTurn>( 1.0 ),
std::make_unique<RightTurn>( 1.0 ),
std::make_unique<LeftTurnAround>( 1.0 ),
std::make_unique<RightTurnAround>( 1.0 ),
std::make_unique<LeftCircle>( 1.0 ),
std::make_unique<RightCircle>( 1.0 ),
std::make_unique<Arc>( 1.0, 2 * M_PI ), // another circle
} );
constexpr double expected_path_length = 1.0 + 2.0 * ( M_PI_2 + M_PI_2 ) + 2.0 * ( M_PI ) +3.0 *
( 2.0 * M_PI );
EXPECT_NEAR( path.poses.size( ), 1 + static_cast<std::size_t>( expected_path_length / spacing ), 50 );
for ( const auto & pose : path.poses ) {
EXPECT_EQ( pose.header.frame_id, start.header.frame_id );
}
// Check the last pose
auto & last_pose = path.poses.back( );
auto & last_position = last_pose.pose.position;
EXPECT_NEAR( last_position.x, 3.0, 0.5 );
EXPECT_NEAR( last_position.y, 6.0, 0.5 );
EXPECT_DOUBLE_EQ( last_position.z, 0.0 );
auto & last_orientation = last_pose.pose.orientation;
EXPECT_NEAR( last_orientation.x, 0.0, 0.1 );
EXPECT_NEAR( last_orientation.y, 0.0, 0.1 );
EXPECT_NEAR( last_orientation.z, 0.0, 0.1 );
EXPECT_NEAR( last_orientation.w, 1.0, 0.1 );
}
// Copyright ( c ) 2021 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <math.h>
#include <memory>
#include <string>
#include <vector>
#include <limits>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "path_utils/path_utils.hpp"
#include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp"
#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
#include "nav2_core/exceptions.hpp"
30 class RclCppFixture
{
public:
33 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
34 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
38 class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
{
public:
41 BasicAPIRPP( )
: nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController( ) {}
44 nav_msgs::msg::Path getPlan( ) {return global_plan_;}
46 double getSpeed( ) {return desired_linear_vel_;}
48 std::unique_ptr<geometry_msgs::msg::PointStamped> createCarrotMsgWrapper(
49 const geometry_msgs::msg::PoseStamped & carrot_pose )
{
return createCarrotMsg( carrot_pose );
}
54 void setVelocityScaledLookAhead( ) {use_velocity_scaled_lookahead_dist_ = true;}
55 void setCostRegulationScaling( ) {use_cost_regulated_linear_velocity_scaling_ = true;}
56 void resetVelocityRegulationScaling( ) {use_regulated_linear_velocity_scaling_ = false;}
58 double getLookAheadDistanceWrapper( const geometry_msgs::msg::Twist & twist )
{
return getLookAheadDistance( twist );
}
63 static geometry_msgs::msg::Point circleSegmentIntersectionWrapper(
64 const geometry_msgs::msg::Point & p1,
65 const geometry_msgs::msg::Point & p2,
double r )
{
return circleSegmentIntersection( p1, p2, r );
}
71 geometry_msgs::msg::PoseStamped getLookAheadPointWrapper(
72 const double & dist, const nav_msgs::msg::Path & path )
{
return getLookAheadPoint( dist, path );
}
77 bool shouldRotateToPathWrapper(
78 const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path )
{
return shouldRotateToPath( carrot_pose, angle_to_path );
}
83 bool shouldRotateToGoalHeadingWrapper( const geometry_msgs::msg::PoseStamped & carrot_pose )
{
return shouldRotateToGoalHeading( carrot_pose );
}
88 void rotateToHeadingWrapper(
double & linear_vel, double & angular_vel,
90 const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed )
{
return rotateToHeading( linear_vel, angular_vel, angle_to_path, curr_speed );
}
95 void applyConstraintsWrapper(
const double & dist_error, const double & lookahead_dist,
97 const double & curvature, const geometry_msgs::msg::Twist & curr_speed,
const double & pose_cost, double & linear_vel, double & sign )
{
return applyConstraints(
dist_error, lookahead_dist, curvature, curr_speed, pose_cost,
linear_vel, sign );
}
105 double findVelocitySignChangeWrapper(
106 const nav_msgs::msg::Path & transformed_plan )
{
return findVelocitySignChange( transformed_plan );
}
111 nav_msgs::msg::Path transformGlobalPlanWrapper(
112 const geometry_msgs::msg::PoseStamped & pose )
{
return transformGlobalPlan( pose );
}
};
118 TEST( RegulatedPurePursuitTest, basicAPI )
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>( "testRPP" );
std::string name = "PathFollower";
auto tf = std::make_shared<tf2_ros::Buffer>( node->get_clock( ) );
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "fake_costmap" );
// instantiate
auto ctrl = std::make_shared<BasicAPIRPP>( );
costmap->on_configure( rclcpp_lifecycle::State( ) );
ctrl->configure( node, name, tf, costmap );
ctrl->activate( );
ctrl->deactivate( );
ctrl->cleanup( );
// setPlan and get plan
nav_msgs::msg::Path path;
path.poses.resize( 2 );
path.poses[0].header.frame_id = "fake_frame";
ctrl->setPlan( path );
EXPECT_EQ( ctrl->getPlan( ).poses.size( ), 2ul );
EXPECT_EQ( ctrl->getPlan( ).poses[0].header.frame_id, std::string( "fake_frame" ) );
// set speed limit
const double base_speed = ctrl->getSpeed( );
EXPECT_EQ( ctrl->getSpeed( ), base_speed );
ctrl->setSpeedLimit( 0.51, false );
EXPECT_EQ( ctrl->getSpeed( ), 0.51 );
ctrl->setSpeedLimit( nav2_costmap_2d::NO_SPEED_LIMIT, false );
EXPECT_EQ( ctrl->getSpeed( ), base_speed );
ctrl->setSpeedLimit( 30, true );
EXPECT_EQ( ctrl->getSpeed( ), base_speed * 0.3 );
ctrl->setSpeedLimit( nav2_costmap_2d::NO_SPEED_LIMIT, true );
EXPECT_EQ( ctrl->getSpeed( ), base_speed );
}
154 TEST( RegulatedPurePursuitTest, createCarrotMsg )
{
auto ctrl = std::make_shared<BasicAPIRPP>( );
geometry_msgs::msg::PoseStamped pose;
pose.header.frame_id = "Hi!";
pose.pose.position.x = 1.0;
pose.pose.position.y = 12.0;
pose.pose.orientation.w = 0.5;
auto rtn = ctrl->createCarrotMsgWrapper( pose );
EXPECT_EQ( rtn->header.frame_id, std::string( "Hi!" ) );
EXPECT_EQ( rtn->point.x, 1.0 );
EXPECT_EQ( rtn->point.y, 12.0 );
EXPECT_EQ( rtn->point.z, 0.01 );
}
170 TEST( RegulatedPurePursuitTest, findVelocitySignChange )
{
auto ctrl = std::make_shared<BasicAPIRPP>( );
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>( "testRPPfindVelocitySignChange" );
geometry_msgs::msg::PoseStamped pose;
pose.header.frame_id = "smb";
auto time = node->get_clock( )->now( );
pose.header.stamp = time;
pose.pose.position.x = 1.0;
pose.pose.position.y = 0.0;
nav_msgs::msg::Path path;
path.poses.resize( 3 );
path.header.frame_id = "smb";
path.header.stamp = pose.header.stamp;
path.poses[0].pose.position.x = 1.0;
path.poses[0].pose.position.y = 1.0;
path.poses[1].pose.position.x = 2.0;
path.poses[1].pose.position.y = 2.0;
path.poses[2].pose.position.x = -1.0;
path.poses[2].pose.position.y = -1.0;
ctrl->setPlan( path );
auto rtn = ctrl->findVelocitySignChangeWrapper( path );
EXPECT_EQ( rtn, sqrt( 8.0 ) );
path.poses[2].pose.position.x = 3.0;
path.poses[2].pose.position.y = 3.0;
ctrl->setPlan( path );
rtn = ctrl->findVelocitySignChangeWrapper( path );
EXPECT_EQ( rtn, std::numeric_limits<double>::max( ) );
}
using CircleSegmentIntersectionParam = std::tuple<
std::pair<double, double>,
std::pair<double, double>,
double,
std::pair<double, double>
>;
209 class CircleSegmentIntersectionTest
210 : public ::testing::TestWithParam<CircleSegmentIntersectionParam>
{};
213 TEST_P( CircleSegmentIntersectionTest, circleSegmentIntersection )
{
auto pair1 = std::get<0>( GetParam( ) );
auto pair2 = std::get<1>( GetParam( ) );
auto r = std::get<2>( GetParam( ) );
auto expected_pair = std::get<3>( GetParam( ) );
auto pair_to_point = []( std::pair<double, double> p ) -> geometry_msgs::msg::Point {
geometry_msgs::msg::Point point;
point.x = p.first;
point.y = p.second;
point.z = 0.0;
return point;
};
auto p1 = pair_to_point( pair1 );
auto p2 = pair_to_point( pair2 );
auto actual = BasicAPIRPP::circleSegmentIntersectionWrapper( p1, p2, r );
auto expected_point = pair_to_point( expected_pair );
EXPECT_DOUBLE_EQ( actual.x, expected_point.x );
EXPECT_DOUBLE_EQ( actual.y, expected_point.y );
// Expect that the intersection point is actually r away from the origin
EXPECT_DOUBLE_EQ( r, std::hypot( actual.x, actual.y ) );
}
236 INSTANTIATE_TEST_SUITE_P(
InterpolationTest,
CircleSegmentIntersectionTest,
testing::Values(
// Origin to the positive X axis
CircleSegmentIntersectionParam{
{0.0, 0.0},
{2.0, 0.0},
1.0,
{1.0, 0.0}
},
// Origin to hte negative X axis
CircleSegmentIntersectionParam{
{0.0, 0.0},
{-2.0, 0.0},
1.0,
{-1.0, 0.0}
},
// Origin to the positive Y axis
CircleSegmentIntersectionParam{
{0.0, 0.0},
{0.0, 2.0},
1.0,
{0.0, 1.0}
},
// Origin to the negative Y axis
CircleSegmentIntersectionParam{
{0.0, 0.0},
{0.0, -2.0},
1.0,
{0.0, -1.0}
},
// non-origin to the X axis with non-unit circle, with the second point inside
CircleSegmentIntersectionParam{
{4.0, 0.0},
{-1.0, 0.0},
2.0,
{2.0, 0.0}
},
// non-origin to the Y axis with non-unit circle, with the second point inside
CircleSegmentIntersectionParam{
{0.0, 4.0},
{0.0, -0.5},
2.0,
{0.0, 2.0}
},
// origin to the positive X axis, on the circle
CircleSegmentIntersectionParam{
{2.0, 0.0},
{0.0, 0.0},
2.0,
{2.0, 0.0}
},
// origin to the positive Y axis, on the circle
CircleSegmentIntersectionParam{
{0.0, 0.0},
{0.0, 2.0},
2.0,
{0.0, 2.0}
},
// origin to the upper-right quadrant ( 3-4-5 triangle )
CircleSegmentIntersectionParam{
{0.0, 0.0},
{6.0, 8.0},
5.0,
{3.0, 4.0}
},
// origin to the lower-left quadrant ( 3-4-5 triangle )
CircleSegmentIntersectionParam{
{0.0, 0.0},
{-6.0, -8.0},
5.0,
{-3.0, -4.0}
},
// origin to the upper-left quadrant ( 3-4-5 triangle )
CircleSegmentIntersectionParam{
{0.0, 0.0},
{-6.0, 8.0},
5.0,
{-3.0, 4.0}
},
// origin to the lower-right quadrant ( 3-4-5 triangle )
CircleSegmentIntersectionParam{
{0.0, 0.0},
{6.0, -8.0},
5.0,
{3.0, -4.0}
}
) );
326 TEST( RegulatedPurePursuitTest, lookaheadAPI )
{
auto ctrl = std::make_shared<BasicAPIRPP>( );
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>( "testRPP" );
std::string name = "PathFollower";
auto tf = std::make_shared<tf2_ros::Buffer>( node->get_clock( ) );
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "fake_costmap" );
rclcpp_lifecycle::State state;
costmap->on_configure( state );
ctrl->configure( node, name, tf, costmap );
geometry_msgs::msg::Twist twist;
// test getLookAheadDistance
double rtn = ctrl->getLookAheadDistanceWrapper( twist );
EXPECT_EQ( rtn, 0.6 ); // default lookahead_dist
// shouldn't be a function of speed
twist.linear.x = 10.0;
rtn = ctrl->getLookAheadDistanceWrapper( twist );
EXPECT_EQ( rtn, 0.6 );
// now it should be a function of velocity, max out
ctrl->setVelocityScaledLookAhead( );
rtn = ctrl->getLookAheadDistanceWrapper( twist );
EXPECT_EQ( rtn, 0.9 ); // 10 speed maxes out at max_lookahead_dist
// check normal range
twist.linear.x = 0.35;
rtn = ctrl->getLookAheadDistanceWrapper( twist );
EXPECT_NEAR( rtn, 0.525, 0.0001 ); // 1.5 * 0.35
// check minimum range
twist.linear.x = 0.0;
rtn = ctrl->getLookAheadDistanceWrapper( twist );
EXPECT_EQ( rtn, 0.3 );
// test getLookAheadPoint
double dist = 1.0;
nav_msgs::msg::Path path;
path.poses.resize( 10 );
for ( uint i = 0; i != path.poses.size( ); i++ ) {
path.poses[i].pose.position.x = static_cast<double>( i );
}
// test exact hits
auto pt = ctrl->getLookAheadPointWrapper( dist, path );
EXPECT_EQ( pt.pose.position.x, 1.0 );
// test getting next closest point without interpolation
node->set_parameter(
rclcpp::Parameter(
name + ".use_interpolation",
rclcpp::ParameterValue( false ) ) );
ctrl->configure( node, name, tf, costmap );
dist = 3.8;
pt = ctrl->getLookAheadPointWrapper( dist, path );
EXPECT_EQ( pt.pose.position.x, 4.0 );
// test end of path
dist = 100.0;
pt = ctrl->getLookAheadPointWrapper( dist, path );
EXPECT_EQ( pt.pose.position.x, 9.0 );
// test interpolation
node->set_parameter(
rclcpp::Parameter(
name + ".use_interpolation",
rclcpp::ParameterValue( true ) ) );
ctrl->configure( node, name, tf, costmap );
dist = 3.8;
pt = ctrl->getLookAheadPointWrapper( dist, path );
EXPECT_EQ( pt.pose.position.x, 3.8 );
}
401 TEST( RegulatedPurePursuitTest, rotateTests )
{
auto ctrl = std::make_shared<BasicAPIRPP>( );
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>( "testRPP" );
std::string name = "PathFollower";
auto tf = std::make_shared<tf2_ros::Buffer>( node->get_clock( ) );
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "fake_costmap" );
rclcpp_lifecycle::State state;
costmap->on_configure( state );
ctrl->configure( node, name, tf, costmap );
// shouldRotateToPath
geometry_msgs::msg::PoseStamped carrot;
double angle_to_path_rtn;
EXPECT_EQ( ctrl->shouldRotateToPathWrapper( carrot, angle_to_path_rtn ), false );
carrot.pose.position.x = 0.5;
carrot.pose.position.y = 0.25;
EXPECT_EQ( ctrl->shouldRotateToPathWrapper( carrot, angle_to_path_rtn ), false );
carrot.pose.position.x = 0.5;
carrot.pose.position.y = 1.0;
EXPECT_EQ( ctrl->shouldRotateToPathWrapper( carrot, angle_to_path_rtn ), true );
// shouldRotateToGoalHeading
carrot.pose.position.x = 0.0;
carrot.pose.position.y = 0.0;
EXPECT_EQ( ctrl->shouldRotateToGoalHeadingWrapper( carrot ), true );
carrot.pose.position.x = 0.0;
carrot.pose.position.y = 0.24;
EXPECT_EQ( ctrl->shouldRotateToGoalHeadingWrapper( carrot ), true );
carrot.pose.position.x = 0.0;
carrot.pose.position.y = 0.26;
EXPECT_EQ( ctrl->shouldRotateToGoalHeadingWrapper( carrot ), false );
// rotateToHeading
double lin_v = 10.0;
double ang_v = 0.5;
double angle_to_path = 0.4;
geometry_msgs::msg::Twist curr_speed;
curr_speed.angular.z = 1.75;
// basic full speed at a speed
ctrl->rotateToHeadingWrapper( lin_v, ang_v, angle_to_path, curr_speed );
EXPECT_EQ( lin_v, 0.0 );
EXPECT_EQ( ang_v, 1.8 );
// negative direction
angle_to_path = -0.4;
curr_speed.angular.z = -1.75;
ctrl->rotateToHeadingWrapper( lin_v, ang_v, angle_to_path, curr_speed );
EXPECT_EQ( ang_v, -1.8 );
// kinematic clamping, no speed, some speed accelerating, some speed decelerating
angle_to_path = 0.4;
curr_speed.angular.z = 0.0;
ctrl->rotateToHeadingWrapper( lin_v, ang_v, angle_to_path, curr_speed );
EXPECT_NEAR( ang_v, 0.16, 0.01 );
curr_speed.angular.z = 1.0;
ctrl->rotateToHeadingWrapper( lin_v, ang_v, angle_to_path, curr_speed );
EXPECT_NEAR( ang_v, 1.16, 0.01 );
angle_to_path = -0.4;
curr_speed.angular.z = 1.0;
ctrl->rotateToHeadingWrapper( lin_v, ang_v, angle_to_path, curr_speed );
EXPECT_NEAR( ang_v, 0.84, 0.01 );
}
472 TEST( RegulatedPurePursuitTest, applyConstraints )
{
auto ctrl = std::make_shared<BasicAPIRPP>( );
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>( "testRPP" );
std::string name = "PathFollower";
auto tf = std::make_shared<tf2_ros::Buffer>( node->get_clock( ) );
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "fake_costmap" );
rclcpp_lifecycle::State state;
costmap->on_configure( state );
ctrl->configure( node, name, tf, costmap );
double dist_error = 0.0;
double lookahead_dist = 0.6;
double curvature = 0.5;
geometry_msgs::msg::Twist curr_speed;
double pose_cost = 0.0;
double linear_vel = 0.0;
double sign = 1.0;
// test curvature regulation ( default )
curr_speed.linear.x = 0.25;
ctrl->applyConstraintsWrapper(
dist_error, lookahead_dist, curvature, curr_speed, pose_cost,
linear_vel, sign );
EXPECT_EQ( linear_vel, 0.25 ); // min set speed
linear_vel = 1.0;
curvature = 0.7407;
curr_speed.linear.x = 0.5;
ctrl->applyConstraintsWrapper(
dist_error, lookahead_dist, curvature, curr_speed, pose_cost,
linear_vel, sign );
EXPECT_NEAR( linear_vel, 0.5, 0.01 ); // lower by curvature
linear_vel = 1.0;
curvature = 1000.0;
curr_speed.linear.x = 0.25;
ctrl->applyConstraintsWrapper(
dist_error, lookahead_dist, curvature, curr_speed, pose_cost,
linear_vel, sign );
EXPECT_NEAR( linear_vel, 0.25, 0.01 ); // min out by curvature
// now try with cost regulation ( turn off velocity and only cost )
// ctrl->setCostRegulationScaling( );
// ctrl->resetVelocityRegulationScaling( );
// curvature = 0.0;
// min changable cost
// pose_cost = 1;
// linear_vel = 0.5;
// curr_speed.linear.x = 0.5;
// ctrl->applyConstraintsWrapper(
// dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel );
// EXPECT_NEAR( linear_vel, 0.498, 0.01 );
// max changing cost
// pose_cost = 127;
// curr_speed.linear.x = 0.255;
// ctrl->applyConstraintsWrapper(
// dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel );
// EXPECT_NEAR( linear_vel, 0.255, 0.01 );
// over max cost thresh
// pose_cost = 200;
// curr_speed.linear.x = 0.25;
// ctrl->applyConstraintsWrapper(
// dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel );
// EXPECT_NEAR( linear_vel, 0.25, 0.01 );
// test kinematic clamping
// pose_cost = 200;
// curr_speed.linear.x = 1.0;
// ctrl->applyConstraintsWrapper(
// dist_error, lookahead_dist, curvature, curr_speed, pose_cost, linear_vel );
// EXPECT_NEAR( linear_vel, 0.5, 0.01 );
}
550 TEST( RegulatedPurePursuitTest, testDynamicParameter )
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>( "Smactest" );
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "global_costmap" );
costmap->on_configure( rclcpp_lifecycle::State( ) );
auto ctrl =
std::make_unique<nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController>( );
auto tf = std::make_shared<tf2_ros::Buffer>( node->get_clock( ) );
ctrl->configure( node, "test", tf, costmap );
ctrl->activate( );
auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
node->get_node_base_interface( ), node->get_node_topics_interface( ),
node->get_node_graph_interface( ),
node->get_node_services_interface( ) );
auto results = rec_param->set_parameters_atomically(
{rclcpp::Parameter( "test.desired_linear_vel", 1.0 ),
rclcpp::Parameter( "test.lookahead_dist", 7.0 ),
rclcpp::Parameter( "test.max_lookahead_dist", 7.0 ),
rclcpp::Parameter( "test.min_lookahead_dist", 6.0 ),
rclcpp::Parameter( "test.lookahead_time", 1.8 ),
rclcpp::Parameter( "test.rotate_to_heading_angular_vel", 18.0 ),
rclcpp::Parameter( "test.min_approach_linear_velocity", 1.0 ),
rclcpp::Parameter( "test.max_allowed_time_to_collision_up_to_carrot", 2.0 ),
rclcpp::Parameter( "test.cost_scaling_dist", 2.0 ),
rclcpp::Parameter( "test.cost_scaling_gain", 4.0 ),
rclcpp::Parameter( "test.regulated_linear_scaling_min_radius", 10.0 ),
rclcpp::Parameter( "test.transform_tolerance", 30.0 ),
rclcpp::Parameter( "test.max_angular_accel", 3.0 ),
rclcpp::Parameter( "test.rotate_to_heading_min_angle", 0.7 ),
rclcpp::Parameter( "test.regulated_linear_scaling_min_speed", 4.0 ),
rclcpp::Parameter( "test.use_velocity_scaled_lookahead_dist", false ),
rclcpp::Parameter( "test.use_regulated_linear_velocity_scaling", false ),
rclcpp::Parameter( "test.use_cost_regulated_linear_velocity_scaling", false ),
rclcpp::Parameter( "test.allow_reversing", false ),
rclcpp::Parameter( "test.use_rotate_to_heading", false )} );
rclcpp::spin_until_future_complete(
node->get_node_base_interface( ),
results );
EXPECT_EQ( node->get_parameter( "test.desired_linear_vel" ).as_double( ), 1.0 );
EXPECT_EQ( node->get_parameter( "test.lookahead_dist" ).as_double( ), 7.0 );
EXPECT_EQ( node->get_parameter( "test.max_lookahead_dist" ).as_double( ), 7.0 );
EXPECT_EQ( node->get_parameter( "test.min_lookahead_dist" ).as_double( ), 6.0 );
EXPECT_EQ( node->get_parameter( "test.lookahead_time" ).as_double( ), 1.8 );
EXPECT_EQ( node->get_parameter( "test.rotate_to_heading_angular_vel" ).as_double( ), 18.0 );
EXPECT_EQ( node->get_parameter( "test.min_approach_linear_velocity" ).as_double( ), 1.0 );
EXPECT_EQ(
node->get_parameter(
"test.max_allowed_time_to_collision_up_to_carrot" ).as_double( ), 2.0 );
EXPECT_EQ( node->get_parameter( "test.cost_scaling_dist" ).as_double( ), 2.0 );
EXPECT_EQ( node->get_parameter( "test.cost_scaling_gain" ).as_double( ), 4.0 );
EXPECT_EQ( node->get_parameter( "test.regulated_linear_scaling_min_radius" ).as_double( ), 10.0 );
EXPECT_EQ( node->get_parameter( "test.transform_tolerance" ).as_double( ), 30.0 );
EXPECT_EQ( node->get_parameter( "test.max_angular_accel" ).as_double( ), 3.0 );
EXPECT_EQ( node->get_parameter( "test.rotate_to_heading_min_angle" ).as_double( ), 0.7 );
EXPECT_EQ( node->get_parameter( "test.regulated_linear_scaling_min_speed" ).as_double( ), 4.0 );
EXPECT_EQ( node->get_parameter( "test.use_velocity_scaled_lookahead_dist" ).as_bool( ), false );
EXPECT_EQ( node->get_parameter( "test.use_regulated_linear_velocity_scaling" ).as_bool( ), false );
EXPECT_EQ(
node->get_parameter(
"test.use_cost_regulated_linear_velocity_scaling" ).as_bool( ), false );
EXPECT_EQ( node->get_parameter( "test.allow_reversing" ).as_bool( ), false );
EXPECT_EQ( node->get_parameter( "test.use_rotate_to_heading" ).as_bool( ), false );
}
618 class TransformGlobalPlanTest : public ::testing::Test
{
protected:
void SetUp( ) override
{
ctrl_ = std::make_shared<BasicAPIRPP>( );
node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>( "testRPP" );
tf_buffer_ = std::make_shared<tf2_ros::Buffer>( node_->get_clock( ) );
costmap_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "fake_costmap" );
}
void configure_costmap( uint16_t width, double resolution )
{
constexpr char costmap_frame[] = "test_costmap_frame";
632 constexpr char robot_frame[] = "test_robot_frame";
634 auto results = costmap_->set_parameters(
{
rclcpp::Parameter( "global_frame", costmap_frame ),
rclcpp::Parameter( "robot_base_frame", robot_frame ),
rclcpp::Parameter( "width", width ),
rclcpp::Parameter( "height", width ),
rclcpp::Parameter( "resolution", resolution )
} );
for ( const auto & result : results ) {
EXPECT_TRUE( result.successful ) << result.reason;
}
rclcpp_lifecycle::State state;
costmap_->on_configure( state );
}
void configure_controller( double max_robot_pose_search_dist )
{
std::string plugin_name = "test_rpp";
nav2_util::declare_parameter_if_not_declared(
node_, plugin_name + ".max_robot_pose_search_dist",
rclcpp::ParameterValue( max_robot_pose_search_dist ) );
ctrl_->configure( node_, plugin_name, tf_buffer_, costmap_ );
}
void setup_transforms( geometry_msgs::msg::Point & robot_position )
{
transform_time_ = node_->get_clock( )->now( );
// Note: transforms go parent to child
// We will have a separate path and costmap frame for completeness,
// but we will leave them cooincident for convenience.
geometry_msgs::msg::TransformStamped path_to_costmap;
path_to_costmap.header.frame_id = PATH_FRAME;
path_to_costmap.header.stamp = transform_time_;
path_to_costmap.child_frame_id = COSTMAP_FRAME;
path_to_costmap.transform.translation.x = 0.0;
path_to_costmap.transform.translation.y = 0.0;
path_to_costmap.transform.translation.z = 0.0;
geometry_msgs::msg::TransformStamped costmap_to_robot;
costmap_to_robot.header.frame_id = COSTMAP_FRAME;
costmap_to_robot.header.stamp = transform_time_;
costmap_to_robot.child_frame_id = ROBOT_FRAME;
costmap_to_robot.transform.translation.x = robot_position.x;
costmap_to_robot.transform.translation.y = robot_position.y;
costmap_to_robot.transform.translation.z = robot_position.z;
tf2_msgs::msg::TFMessage tf_message;
tf_message.transforms = {
path_to_costmap,
costmap_to_robot
};
for ( const auto & transform : tf_message.transforms ) {
tf_buffer_->setTransform( transform, "test", false );
}
tf_buffer_->setUsingDedicatedThread( true ); // lying to let it do transforms
}
static constexpr char PATH_FRAME[] = "test_path_frame";
static constexpr char COSTMAP_FRAME[] = "test_costmap_frame";
static constexpr char ROBOT_FRAME[] = "test_robot_frame";
std::shared_ptr<BasicAPIRPP> ctrl_;
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_;
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
rclcpp::Time transform_time_;
};
// This tests that not only should nothing get pruned on a costmap
// that contains the entire global_plan, and also that it doesn't skip to the end of the path
// which is closer to the robot pose than the start.
TEST_F( TransformGlobalPlanTest, no_pruning_on_large_costmap )
{
geometry_msgs::msg::PoseStamped robot_pose;
robot_pose.header.frame_id = COSTMAP_FRAME;
robot_pose.header.stamp = transform_time_;
robot_pose.pose.position.x = -0.1;
robot_pose.pose.position.y = 0.0;
robot_pose.pose.position.z = 0.0;
// A really big costmap
// the max_costmap_extent should be 50m
configure_costmap( 100u, 0.1 );
configure_controller( 5.0 );
setup_transforms( robot_pose.pose.position );
// Set up test path;
geometry_msgs::msg::PoseStamped start_of_path;
start_of_path.header.frame_id = PATH_FRAME;
start_of_path.header.stamp = transform_time_;
start_of_path.pose.position.x = 0.0;
start_of_path.pose.position.y = 0.0;
start_of_path.pose.position.z = 0.0;
constexpr double spacing = 0.1;
constexpr double circle_radius = 1.0;
auto global_plan = path_utils::generate_path(
start_of_path, spacing, {
std::make_unique<path_utils::LeftCircle>( circle_radius )
} );
ctrl_->setPlan( global_plan );
// Transform the plan
auto transformed_plan = ctrl_->transformGlobalPlanWrapper( robot_pose );
EXPECT_EQ( transformed_plan.poses.size( ), global_plan.poses.size( ) );
}
// This plan shouldn't get pruned because of the costmap,
// but should be half pruned because it is halfway around the circle
TEST_F( TransformGlobalPlanTest, transform_start_selection )
{
geometry_msgs::msg::PoseStamped robot_pose;
robot_pose.header.frame_id = COSTMAP_FRAME;
robot_pose.header.stamp = transform_time_;
robot_pose.pose.position.x = 0.0;
robot_pose.pose.position.y = 4.0; // on the other side of the circle
robot_pose.pose.position.z = 0.0;
// Could set orientation going the other way, but RPP doesn't care
constexpr double spacing = 0.1;
constexpr double circle_radius = 2.0; // diameter 4
// A really big costmap
// the max_costmap_extent should be 50m
configure_costmap( 100u, 0.1 );
// This should just be at least half the circumference: pi*r ~= 6
constexpr double max_robot_pose_search_dist = 10.0;
configure_controller( max_robot_pose_search_dist );
setup_transforms( robot_pose.pose.position );
// Set up test path;
geometry_msgs::msg::PoseStamped start_of_path;
start_of_path.header.frame_id = PATH_FRAME;
start_of_path.header.stamp = transform_time_;
start_of_path.pose.position.x = 0.0;
start_of_path.pose.position.y = 0.0;
start_of_path.pose.position.z = 0.0;
auto global_plan = path_utils::generate_path(
start_of_path, spacing, {
std::make_unique<path_utils::LeftCircle>( circle_radius )
} );
ctrl_->setPlan( global_plan );
// Transform the plan
auto transformed_plan = ctrl_->transformGlobalPlanWrapper( robot_pose );
EXPECT_NEAR( transformed_plan.poses.size( ), global_plan.poses.size( ) / 2, 1 );
EXPECT_NEAR( transformed_plan.poses[0].pose.position.x, 0.0, 0.5 );
EXPECT_NEAR( transformed_plan.poses[0].pose.position.y, 0.0, 0.5 );
}
// This should throw an exception when all poses are outside of the costmap
TEST_F( TransformGlobalPlanTest, all_poses_outside_of_costmap )
{
geometry_msgs::msg::PoseStamped robot_pose;
robot_pose.header.frame_id = COSTMAP_FRAME;
robot_pose.header.stamp = transform_time_;
// far away from the path
robot_pose.pose.position.x = 1000.0;
robot_pose.pose.position.y = 1000.0;
robot_pose.pose.position.z = 0.0;
// Could set orientation going the other way, but RPP doesn't care
constexpr double spacing = 0.1;
constexpr double circle_radius = 2.0; // diameter 4
// A "normal" costmap
// the max_costmap_extent should be 50m
configure_costmap( 10u, 0.1 );
// This should just be at least half the circumference: pi*r ~= 6
constexpr double max_robot_pose_search_dist = 10.0;
configure_controller( max_robot_pose_search_dist );
setup_transforms( robot_pose.pose.position );
// Set up test path;
geometry_msgs::msg::PoseStamped start_of_path;
start_of_path.header.frame_id = PATH_FRAME;
start_of_path.header.stamp = transform_time_;
start_of_path.pose.position.x = 0.0;
start_of_path.pose.position.y = 0.0;
start_of_path.pose.position.z = 0.0;
auto global_plan = path_utils::generate_path(
start_of_path, spacing, {
std::make_unique<path_utils::LeftCircle>( circle_radius )
} );
ctrl_->setPlan( global_plan );
// Transform the plan
EXPECT_THROW( ctrl_->transformGlobalPlanWrapper( robot_pose ), nav2_core::PlannerException );
}
// Should shortcut the circle if the circle is shorter than max_robot_pose_search_dist
TEST_F( TransformGlobalPlanTest, good_circle_shortcut )
{
geometry_msgs::msg::PoseStamped robot_pose;
robot_pose.header.frame_id = COSTMAP_FRAME;
robot_pose.header.stamp = transform_time_;
// far away from the path
robot_pose.pose.position.x = -0.1;
robot_pose.pose.position.y = 0.0;
robot_pose.pose.position.z = 0.0;
// Could set orientation going the other way, but RPP doesn't care
constexpr double spacing = 0.1;
constexpr double circle_radius = 2.0; // diameter 4
// A "normal" costmap
// the max_costmap_extent should be 50m
configure_costmap( 100u, 0.1 );
// This should just be at least the circumference: 2*pi*r ~= 12
constexpr double max_robot_pose_search_dist = 15.0;
configure_controller( max_robot_pose_search_dist );
setup_transforms( robot_pose.pose.position );
// Set up test path;
geometry_msgs::msg::PoseStamped start_of_path;
start_of_path.header.frame_id = PATH_FRAME;
start_of_path.header.stamp = transform_time_;
start_of_path.pose.position.x = 0.0;
start_of_path.pose.position.y = 0.0;
start_of_path.pose.position.z = 0.0;
auto global_plan = path_utils::generate_path(
start_of_path, spacing, {
std::make_unique<path_utils::LeftCircle>( circle_radius )
} );
ctrl_->setPlan( global_plan );
// Transform the plan
auto transformed_plan = ctrl_->transformGlobalPlanWrapper( robot_pose );
EXPECT_NEAR( transformed_plan.poses.size( ), 1, 1 );
EXPECT_NEAR( transformed_plan.poses[0].pose.position.x, 0.0, 0.5 );
EXPECT_NEAR( transformed_plan.poses[0].pose.position.y, 0.0, 0.5 );
}
// Simple costmap pruning on a straight line
TEST_F( TransformGlobalPlanTest, costmap_pruning )
{
geometry_msgs::msg::PoseStamped robot_pose;
robot_pose.header.frame_id = COSTMAP_FRAME;
robot_pose.header.stamp = transform_time_;
// far away from the path
robot_pose.pose.position.x = -0.1;
robot_pose.pose.position.y = 0.0;
robot_pose.pose.position.z = 0.0;
// Could set orientation going the other way, but RPP doesn't care
constexpr double spacing = 1.0;
// A "normal" costmap
// the max_costmap_extent should be 50m
configure_costmap( 20u, 0.5 );
constexpr double max_robot_pose_search_dist = 10.0;
configure_controller( max_robot_pose_search_dist );
setup_transforms( robot_pose.pose.position );
// Set up test path;
geometry_msgs::msg::PoseStamped start_of_path;
start_of_path.header.frame_id = PATH_FRAME;
start_of_path.header.stamp = transform_time_;
start_of_path.pose.position.x = 0.0;
start_of_path.pose.position.y = 0.0;
start_of_path.pose.position.z = 0.0;
constexpr double path_length = 100.0;
auto global_plan = path_utils::generate_path(
start_of_path, spacing, {
std::make_unique<path_utils::Straight>( path_length )
} );
ctrl_->setPlan( global_plan );
// Transform the plan
auto transformed_plan = ctrl_->transformGlobalPlanWrapper( robot_pose );
EXPECT_NEAR( transformed_plan.poses.size( ), 10u, 1 );
EXPECT_NEAR( transformed_plan.poses[0].pose.position.x, 0.0, 0.5 );
EXPECT_NEAR( transformed_plan.poses[0].pose.position.y, 0.0, 0.5 );
}
// Should prune out later portions of the path that come back into the costmap
TEST_F( TransformGlobalPlanTest, prune_after_leaving_costmap )
{
geometry_msgs::msg::PoseStamped robot_pose;
robot_pose.header.frame_id = COSTMAP_FRAME;
robot_pose.header.stamp = transform_time_;
// far away from the path
robot_pose.pose.position.x = -0.1;
robot_pose.pose.position.y = 0.0;
robot_pose.pose.position.z = 0.0;
// Could set orientation going the other way, but RPP doesn't care
constexpr double spacing = 1.0;
// A "normal" costmap
// the max_costmap_extent should be 50m
configure_costmap( 20u, 0.5 );
constexpr double max_robot_pose_search_dist = 10.0;
configure_controller( max_robot_pose_search_dist );
setup_transforms( robot_pose.pose.position );
// Set up test path;
geometry_msgs::msg::PoseStamped start_of_path;
start_of_path.header.frame_id = PATH_FRAME;
start_of_path.header.stamp = transform_time_;
start_of_path.pose.position.x = 0.0;
start_of_path.pose.position.y = 0.0;
start_of_path.pose.position.z = 0.0;
constexpr double path_length = 100.0;
auto global_plan = path_utils::generate_path(
start_of_path, spacing, {
std::make_unique<path_utils::Straight>( path_length ),
std::make_unique<path_utils::LeftTurnAround>( 1.0 ),
std::make_unique<path_utils::Straight>( path_length )
} );
ctrl_->setPlan( global_plan );
// Transform the plan
auto transformed_plan = ctrl_->transformGlobalPlanWrapper( robot_pose );
// This should be essentially the same as the regular straight path
EXPECT_NEAR( transformed_plan.poses.size( ), 10u, 1 );
EXPECT_NEAR( transformed_plan.poses[0].pose.position.x, 0.0, 0.5 );
EXPECT_NEAR( transformed_plan.poses[0].pose.position.y, 0.0, 0.5 );
}
// Copyright ( c ) 2021 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_ROTATION_SHIM_CONTROLLER__NAV2_ROTATION_SHIM_CONTROLLER_HPP_
#define NAV2_ROTATION_SHIM_CONTROLLER__NAV2_ROTATION_SHIM_CONTROLLER_HPP_
#include <string>
#include <vector>
#include <memory>
#include <algorithm>
#include <mutex>
#include "rclcpp/rclcpp.hpp"
#include "pluginlib/class_loader.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav2_core/controller.hpp"
#include "nav2_core/exceptions.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_costmap_2d/footprint_collision_checker.hpp"
#include "angles/angles.h"
namespace nav2_rotation_shim_controller
{
/**
* @class nav2_rotation_shim_controller::RotationShimController
* @brief Rotate to rough path heading controller shim plugin
*/
42 class RotationShimController : public nav2_core::Controller
{
public:
/**
* @brief Constructor for nav2_rotation_shim_controller::RotationShimController
*/
48 RotationShimController( );
/**
* @brief Destrructor for nav2_rotation_shim_controller::RotationShimController
*/
~RotationShimController( ) override = default;
/**
* @brief Configure controller state machine
* @param parent WeakPtr to node
* @param name Name of plugin
* @param tf TF buffer
* @param costmap_ros Costmap2DROS object of environment
*/
void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros ) override;
/**
* @brief Cleanup controller state machine
*/
void cleanup( ) override;
/**
* @brief Activate controller state machine
*/
void activate( ) override;
/**
* @brief Deactivate controller state machine
*/
void deactivate( ) override;
/**
* @brief Compute the best command given the current pose and velocity
* @param pose Current robot pose
* @param velocity Current robot velocity
* @param goal_checker Ptr to the goal checker for this task in case useful in computing commands
* @return Best command
*/
geometry_msgs::msg::TwistStamped computeVelocityCommands(
const geometry_msgs::msg::PoseStamped & pose,
const geometry_msgs::msg::Twist & velocity,
nav2_core::GoalChecker * /*goal_checker*/ ) override;
/**
* @brief nav2_core setPlan - Sets the global plan
* @param path The global plan
*/
void setPlan( const nav_msgs::msg::Path & path ) override;
/**
* @brief Limits the maximum linear speed of the robot.
* @param speed_limit expressed in absolute value ( in m/s )
* or in percentage from maximum robot speed.
* @param percentage Setting speed limit in percentage if true
* or in absolute values in false case.
*/
void setSpeedLimit( const double & speed_limit, const bool & percentage ) override;
protected:
/**
* @brief Finds the point on the path that is roughly the sampling
* point distance away from the robot for use.
* May throw exception if a point at least that far away cannot be found
* @return pt location of the output point
*/
geometry_msgs::msg::PoseStamped getSampledPathPt( );
/**
* @brief Uses TF to find the location of the sampled path point in base frame
* @param pt location of the sampled path point
* @return location of the pose in base frame
*/
geometry_msgs::msg::Pose transformPoseToBaseFrame( const geometry_msgs::msg::PoseStamped & pt );
/**
* @brief Rotates the robot to the rough heading
* @param angular_distance Angular distance to the goal remaining
* @param pose Starting pose of robot
* @param velocity Starting velocity of robot
* @return Twist command for rotation to rough heading
*/
geometry_msgs::msg::TwistStamped computeRotateToHeadingCommand(
const double & angular_distance,
const geometry_msgs::msg::PoseStamped & pose,
const geometry_msgs::msg::Twist & velocity );
/**
* @brief Checks if rotation is safe
* @param cmd_vel Velocity to check over
* @param angular_distance_to_heading Angular distance to heading requested
* @param pose Starting pose of robot
*/
void isCollisionFree(
const geometry_msgs::msg::TwistStamped & cmd_vel,
const double & angular_distance_to_heading,
const geometry_msgs::msg::PoseStamped & pose );
/**
* @brief Callback executed when a parameter change is detected
* @param event ParameterEvent message
*/
rcl_interfaces::msg::SetParametersResult
dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters );
rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
std::shared_ptr<tf2_ros::Buffer> tf_;
std::string plugin_name_;
rclcpp::Logger logger_ {rclcpp::get_logger( "RotationShimController" )};
rclcpp::Clock::SharedPtr clock_;
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
161 std::unique_ptr<nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>
collision_checker_;
pluginlib::ClassLoader<nav2_core::Controller> lp_loader_;
nav2_core::Controller::Ptr primary_controller_;
bool path_updated_;
nav_msgs::msg::Path current_path_;
double forward_sampling_distance_, angular_dist_threshold_;
double rotate_to_heading_angular_vel_, max_angular_accel_;
double control_duration_, simulate_ahead_time_;
// Dynamic parameters handler
std::mutex mutex_;
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
};
} // namespace nav2_rotation_shim_controller
#endif // NAV2_ROTATION_SHIM_CONTROLLER__NAV2_ROTATION_SHIM_CONTROLLER_HPP_
1 // Copyright ( c ) 2021 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <algorithm>
#include <string>
#include <limits>
#include <memory>
#include <vector>
#include <utility>
#include "nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp"
using rcl_interfaces::msg::ParameterType;
namespace nav2_rotation_shim_controller
{
29 RotationShimController::RotationShimController( )
: lp_loader_( "nav2_core", "nav2_core::Controller" ),
primary_controller_( nullptr ),
path_updated_( false )
{
}
36 void RotationShimController::configure(
37 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
38 std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
39 std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros )
{
plugin_name_ = name;
node_ = parent;
auto node = parent.lock( );
tf_ = tf;
costmap_ros_ = costmap_ros;
logger_ = node->get_logger( );
clock_ = node->get_clock( );
std::string primary_controller;
double control_frequency;
nav2_util::declare_parameter_if_not_declared(
node, plugin_name_ + ".angular_dist_threshold", rclcpp::ParameterValue( 0.785 ) ); // 45 deg
nav2_util::declare_parameter_if_not_declared(
node, plugin_name_ + ".forward_sampling_distance", rclcpp::ParameterValue( 0.5 ) );
nav2_util::declare_parameter_if_not_declared(
node, plugin_name_ + ".rotate_to_heading_angular_vel", rclcpp::ParameterValue( 1.8 ) );
nav2_util::declare_parameter_if_not_declared(
node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue( 3.2 ) );
nav2_util::declare_parameter_if_not_declared(
node, plugin_name_ + ".simulate_ahead_time", rclcpp::ParameterValue( 1.0 ) );
nav2_util::declare_parameter_if_not_declared(
node, plugin_name_ + ".primary_controller", rclcpp::PARAMETER_STRING );
node->get_parameter( plugin_name_ + ".angular_dist_threshold", angular_dist_threshold_ );
node->get_parameter( plugin_name_ + ".forward_sampling_distance", forward_sampling_distance_ );
node->get_parameter(
plugin_name_ + ".rotate_to_heading_angular_vel",
rotate_to_heading_angular_vel_ );
node->get_parameter( plugin_name_ + ".max_angular_accel", max_angular_accel_ );
node->get_parameter( plugin_name_ + ".simulate_ahead_time", simulate_ahead_time_ );
primary_controller = node->get_parameter( plugin_name_ + ".primary_controller" ).as_string( );
node->get_parameter( "controller_frequency", control_frequency );
control_duration_ = 1.0 / control_frequency;
try {
primary_controller_ = lp_loader_.createUniqueInstance( primary_controller );
RCLCPP_INFO(
logger_, "Created internal controller for rotation shimming: %s of type %s",
plugin_name_.c_str( ), primary_controller.c_str( ) );
} catch ( const pluginlib::PluginlibException & ex ) {
RCLCPP_FATAL(
logger_,
"Failed to create internal controller for rotation shimming. Exception: %s", ex.what( ) );
return;
}
primary_controller_->configure( parent, name, tf, costmap_ros );
// initialize collision checker and set costmap
collision_checker_ = std::make_unique<nav2_costmap_2d::
FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>( costmap_ros->getCostmap( ) );
}
96 void RotationShimController::activate( )
{
RCLCPP_INFO(
logger_,
"Activating controller: %s of type "
"nav2_rotation_shim_controller::RotationShimController",
plugin_name_.c_str( ) );
primary_controller_->activate( );
auto node = node_.lock( );
dyn_params_handler_ = node->add_on_set_parameters_callback(
std::bind(
&RotationShimController::dynamicParametersCallback,
this, std::placeholders::_1 ) );
}
113 void RotationShimController::deactivate( )
{
RCLCPP_INFO(
logger_,
"Deactivating controller: %s of type "
"nav2_rotation_shim_controller::RotationShimController",
plugin_name_.c_str( ) );
primary_controller_->deactivate( );
dyn_params_handler_.reset( );
}
126 void RotationShimController::cleanup( )
{
RCLCPP_INFO(
logger_,
"Cleaning up controller: %s of type "
"nav2_rotation_shim_controller::RotationShimController",
plugin_name_.c_str( ) );
primary_controller_->cleanup( );
primary_controller_.reset( );
}
138 geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands(
139 const geometry_msgs::msg::PoseStamped & pose,
140 const geometry_msgs::msg::Twist & velocity,
141 nav2_core::GoalChecker * goal_checker )
{
if ( path_updated_ ) {
std::lock_guard<std::mutex> lock_reinit( mutex_ );
try {
geometry_msgs::msg::Pose sampled_pt_base = transformPoseToBaseFrame( getSampledPathPt( ) );
double angular_distance_to_heading =
std::atan2( sampled_pt_base.position.y, sampled_pt_base.position.x );
if ( fabs( angular_distance_to_heading ) > angular_dist_threshold_ ) {
RCLCPP_DEBUG(
logger_,
"Robot is not within the new path's rough heading, rotating to heading..." );
return computeRotateToHeadingCommand( angular_distance_to_heading, pose, velocity );
} else {
RCLCPP_DEBUG(
logger_,
"Robot is at the new path's rough heading, passing to controller" );
path_updated_ = false;
}
} catch ( const std::runtime_error & e ) {
RCLCPP_DEBUG(
logger_,
"Rotation Shim Controller was unable to find a sampling point, "
" a rotational collision was detected, or TF failed to transform"
" into base frame! what( ): %s", e.what( ) );
path_updated_ = false;
}
}
// If at this point, use the primary controller to path track
return primary_controller_->computeVelocityCommands( pose, velocity, goal_checker );
}
175 geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt( )
{
if ( current_path_.poses.size( ) < 2 ) {
throw nav2_core::PlannerException(
"Path is too short to find a valid sampled path point for rotation." );
}
geometry_msgs::msg::Pose start = current_path_.poses.front( ).pose;
double dx, dy;
// Find the first point at least sampling distance away
for ( unsigned int i = 1; i != current_path_.poses.size( ); i++ ) {
dx = current_path_.poses[i].pose.position.x - start.position.x;
dy = current_path_.poses[i].pose.position.y - start.position.y;
if ( hypot( dx, dy ) >= forward_sampling_distance_ ) {
current_path_.poses[i].header.frame_id = current_path_.header.frame_id;
current_path_.poses[i].header.stamp = clock_->now( ); // Get current time transformation
return current_path_.poses[i];
}
}
throw nav2_core::PlannerException(
std::string(
"Unable to find a sampling point at least %0.2f from the robot, "
"passing off to primary controller plugin.", forward_sampling_distance_ ) );
}
geometry_msgs::msg::Pose
203 RotationShimController::transformPoseToBaseFrame( const geometry_msgs::msg::PoseStamped & pt )
{
geometry_msgs::msg::PoseStamped pt_base;
if ( !nav2_util::transformPoseInTargetFrame( pt, pt_base, *tf_, costmap_ros_->getBaseFrameID( ) ) ) {
throw nav2_core::PlannerException( "Failed to transform pose to base frame!" );
}
return pt_base.pose;
}
geometry_msgs::msg::TwistStamped
213 RotationShimController::computeRotateToHeadingCommand(
const double & angular_distance_to_heading,
215 const geometry_msgs::msg::PoseStamped & pose,
216 const geometry_msgs::msg::Twist & velocity )
{
geometry_msgs::msg::TwistStamped cmd_vel;
cmd_vel.header = pose.header;
const double sign = angular_distance_to_heading > 0.0 ? 1.0 : -1.0;
const double angular_vel = sign * rotate_to_heading_angular_vel_;
const double & dt = control_duration_;
const double min_feasible_angular_speed = velocity.angular.z - max_angular_accel_ * dt;
const double max_feasible_angular_speed = velocity.angular.z + max_angular_accel_ * dt;
cmd_vel.twist.angular.z =
std::clamp( angular_vel, min_feasible_angular_speed, max_feasible_angular_speed );
isCollisionFree( cmd_vel, angular_distance_to_heading, pose );
return cmd_vel;
}
232 void RotationShimController::isCollisionFree(
233 const geometry_msgs::msg::TwistStamped & cmd_vel,
const double & angular_distance_to_heading,
235 const geometry_msgs::msg::PoseStamped & pose )
{
// Simulate rotation ahead by time in control frequency increments
double simulated_time = 0.0;
double initial_yaw = tf2::getYaw( pose.pose.orientation );
double yaw = 0.0;
double footprint_cost = 0.0;
double remaining_rotation_before_thresh =
fabs( angular_distance_to_heading ) - angular_dist_threshold_;
while ( simulated_time < simulate_ahead_time_ ) {
simulated_time += control_duration_;
yaw = initial_yaw + cmd_vel.twist.angular.z * simulated_time;
// Stop simulating past the point it would be passed onto the primary controller
if ( angles::shortest_angular_distance( yaw, initial_yaw ) >= remaining_rotation_before_thresh ) {
break;
}
using namespace nav2_costmap_2d; // NOLINT
footprint_cost = collision_checker_->footprintCostAtPose(
pose.pose.position.x, pose.pose.position.y,
yaw, costmap_ros_->getRobotFootprint( ) );
if ( footprint_cost == static_cast<double>( NO_INFORMATION ) &&
costmap_ros_->getLayeredCostmap( )->isTrackingUnknown( ) )
{
throw std::runtime_error( "RotationShimController detected a potential collision ahead!" );
}
if ( footprint_cost >= static_cast<double>( LETHAL_OBSTACLE ) ) {
throw std::runtime_error( "RotationShimController detected collision ahead!" );
}
}
}
271 void RotationShimController::setPlan( const nav_msgs::msg::Path & path )
{
path_updated_ = true;
current_path_ = path;
primary_controller_->setPlan( path );
}
278 void RotationShimController::setSpeedLimit( const double & speed_limit, const bool & percentage )
{
primary_controller_->setSpeedLimit( speed_limit, percentage );
}
rcl_interfaces::msg::SetParametersResult
284 RotationShimController::dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters )
{
rcl_interfaces::msg::SetParametersResult result;
std::lock_guard<std::mutex> lock_reinit( mutex_ );
for ( auto parameter : parameters ) {
const auto & type = parameter.get_type( );
const auto & name = parameter.get_name( );
if ( type == ParameterType::PARAMETER_DOUBLE ) {
if ( name == plugin_name_ + ".angular_dist_threshold" ) {
angular_dist_threshold_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".forward_sampling_distance" ) {
forward_sampling_distance_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".rotate_to_heading_angular_vel" ) {
rotate_to_heading_angular_vel_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".max_angular_accel" ) {
max_angular_accel_ = parameter.as_double( );
} else if ( name == plugin_name_ + ".simulate_ahead_time" ) {
simulate_ahead_time_ = parameter.as_double( );
}
}
}
result.successful = true;
return result;
}
} // namespace nav2_rotation_shim_controller
// Register this controller as a nav2_core plugin
315 PLUGINLIB_EXPORT_CLASS(
nav2_rotation_shim_controller::RotationShimController,
nav2_core::Controller )
1 // Copyright ( c ) 2021 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <math.h>
#include <memory>
#include <string>
#include <vector>
#include <limits>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_controller/plugins/simple_goal_checker.hpp"
#include "nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp"
#include "tf2_ros/transform_broadcaster.h"
29 class RclCppFixture
{
public:
32 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
33 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
37 class RotationShimShim : public nav2_rotation_shim_controller::RotationShimController
{
public:
40 RotationShimShim( )
: nav2_rotation_shim_controller::RotationShimController( )
{
}
45 nav2_core::Controller::Ptr getPrimaryController( )
{
return primary_controller_;
}
50 nav_msgs::msg::Path getPath( )
{
return current_path_;
}
55 bool isPathUpdated( )
{
return path_updated_;
}
60 geometry_msgs::msg::PoseStamped getSampledPathPtWrapper( )
{
return getSampledPathPt( );
}
65 geometry_msgs::msg::Pose transformPoseToBaseFrameWrapper( geometry_msgs::msg::PoseStamped pt )
{
return transformPoseToBaseFrame( pt );
}
geometry_msgs::msg::TwistStamped
71 computeRotateToHeadingCommandWrapper(
const double & param,
73 const geometry_msgs::msg::PoseStamped & pose,
74 const geometry_msgs::msg::Twist & velocity )
{
return computeRotateToHeadingCommand( param, pose, velocity );
}
};
80 TEST( RotationShimControllerTest, lifecycleTransitions )
{
auto ctrl = std::make_shared<RotationShimShim>( );
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>( "ShimControllerTest" );
std::string name = "PathFollower";
auto tf = std::make_shared<tf2_ros::Buffer>( node->get_clock( ) );
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "fake_costmap" );
rclcpp_lifecycle::State state;
costmap->on_configure( state );
// Should not populate primary controller, does not exist
EXPECT_THROW( ctrl->configure( node, name, tf, costmap ), std::runtime_error );
EXPECT_EQ( ctrl->getPrimaryController( ), nullptr );
// Add a controller to the setup
auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
node->get_node_base_interface( ), node->get_node_topics_interface( ),
node->get_node_graph_interface( ),
node->get_node_services_interface( ) );
auto results = rec_param->set_parameters_atomically(
{rclcpp::Parameter(
"PathFollower.primary_controller",
std::string( "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" ) )} );
rclcpp::spin_until_future_complete(
node->get_node_base_interface( ),
results );
ctrl->configure( node, name, tf, costmap );
EXPECT_NE( ctrl->getPrimaryController( ), nullptr );
ctrl->activate( );
ctrl->setSpeedLimit( 50.0, 2.0 );
ctrl->deactivate( );
ctrl->cleanup( );
}
118 TEST( RotationShimControllerTest, setPlanAndSampledPointsTests )
{
auto ctrl = std::make_shared<RotationShimShim>( );
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>( "ShimControllerTest" );
std::string name = "PathFollower";
auto tf = std::make_shared<tf2_ros::Buffer>( node->get_clock( ) );
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "fake_costmap" );
rclcpp_lifecycle::State state;
costmap->on_configure( state );
// set a valid primary controller so we can do lifecycle
node->declare_parameter(
"PathFollower.primary_controller",
std::string( "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" ) );
auto controller = std::make_shared<RotationShimShim>( );
controller->configure( node, name, tf, costmap );
controller->activate( );
// Test state update and path setting
nav_msgs::msg::Path path;
path.header.frame_id = "hi mate!";
path.poses.resize( 10 );
path.poses[1].pose.position.x = 0.1;
path.poses[1].pose.position.y = 0.1;
path.poses[2].pose.position.x = 1.0;
path.poses[2].pose.position.y = 1.0;
path.poses[3].pose.position.x = 10.0;
path.poses[3].pose.position.y = 10.0;
EXPECT_EQ( controller->isPathUpdated( ), false );
controller->setPlan( path );
EXPECT_EQ( controller->getPath( ).header.frame_id, std::string( "hi mate!" ) );
EXPECT_EQ( controller->getPath( ).poses.size( ), 10u );
EXPECT_EQ( controller->isPathUpdated( ), true );
// Test getting a sampled point
auto pose = controller->getSampledPathPtWrapper( );
EXPECT_EQ( pose.pose.position.x, 1.0 ); // default forward sampling is 0.5
EXPECT_EQ( pose.pose.position.y, 1.0 );
nav_msgs::msg::Path path_invalid_leng;
controller->setPlan( path_invalid_leng );
EXPECT_THROW( controller->getSampledPathPtWrapper( ), std::runtime_error );
nav_msgs::msg::Path path_invalid_dists;
path.poses.resize( 10 );
controller->setPlan( path_invalid_dists );
EXPECT_THROW( controller->getSampledPathPtWrapper( ), std::runtime_error );
}
168 TEST( RotationShimControllerTest, rotationAndTransformTests )
{
auto ctrl = std::make_shared<RotationShimShim>( );
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>( "ShimControllerTest" );
std::string name = "PathFollower";
auto tf = std::make_shared<tf2_ros::Buffer>( node->get_clock( ) );
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "fake_costmap" );
rclcpp_lifecycle::State state;
costmap->on_configure( state );
// set a valid primary controller so we can do lifecycle
node->declare_parameter(
"PathFollower.primary_controller",
std::string( "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" ) );
auto controller = std::make_shared<RotationShimShim>( );
controller->configure( node, name, tf, costmap );
controller->activate( );
// Test state update and path setting
nav_msgs::msg::Path path;
path.header.frame_id = "fake_frame";
path.poses.resize( 10 );
path.poses[1].pose.position.x = 0.1;
path.poses[1].pose.position.y = 0.1;
path.poses[2].pose.position.x = 1.0;
path.poses[2].pose.position.y = 1.0;
path.poses[3].pose.position.x = 10.0;
path.poses[3].pose.position.y = 10.0;
controller->setPlan( path );
const geometry_msgs::msg::Twist velocity;
EXPECT_EQ(
controller->computeRotateToHeadingCommandWrapper(
0.7, path.poses[0], velocity ).twist.angular.z, 1.8 );
EXPECT_EQ(
controller->computeRotateToHeadingCommandWrapper(
-0.7, path.poses[0], velocity ).twist.angular.z, -1.8 );
EXPECT_EQ(
controller->computeRotateToHeadingCommandWrapper(
0.87, path.poses[0], velocity ).twist.angular.z, 1.8 );
// in base_link, so should pass through values without issue
geometry_msgs::msg::PoseStamped pt;
pt.pose.position.x = 100.0;
pt.header.frame_id = "base_link";
pt.header.stamp = rclcpp::Time( );
auto rtn = controller->transformPoseToBaseFrameWrapper( pt );
EXPECT_EQ( rtn.position.x, 100.0 );
// in frame that doesn't exist, shouldn't throw, but should fail
geometry_msgs::msg::PoseStamped pt2;
pt.pose.position.x = 100.0;
pt.header.frame_id = "fake_frame2";
pt.header.stamp = rclcpp::Time( );
EXPECT_THROW( controller->transformPoseToBaseFrameWrapper( pt2 ), std::runtime_error );
}
227 TEST( RotationShimControllerTest, computeVelocityTests )
{
auto ctrl = std::make_shared<RotationShimShim>( );
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>( "ShimControllerTest" );
std::string name = "PathFollower";
auto tf = std::make_shared<tf2_ros::Buffer>( node->get_clock( ) );
auto listener = std::make_shared<tf2_ros::TransformListener>( *tf, node, true );
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "fake_costmap" );
rclcpp_lifecycle::State state;
costmap->on_configure( state );
auto tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>( node );
geometry_msgs::msg::TransformStamped transform;
transform.header.frame_id = "base_link";
transform.child_frame_id = "odom";
transform.transform.rotation.x = 0.0;
transform.transform.rotation.y = 0.0;
transform.transform.rotation.z = 0.0;
transform.transform.rotation.w = 1.0;
tf_broadcaster->sendTransform( transform );
// set a valid primary controller so we can do lifecycle
node->declare_parameter(
"PathFollower.primary_controller",
std::string( "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" ) );
auto controller = std::make_shared<RotationShimShim>( );
controller->configure( node, name, tf, costmap );
controller->activate( );
// Test state update and path setting
nav_msgs::msg::Path path;
path.header.frame_id = "fake_frame";
path.poses.resize( 10 );
geometry_msgs::msg::PoseStamped pose;
pose.header.frame_id = "base_link";
geometry_msgs::msg::Twist velocity;
nav2_controller::SimpleGoalChecker checker;
checker.initialize( node, "checker", costmap );
// send without setting a path - should go to RPP immediately
// then it should throw an exception because the path is empty and invalid
EXPECT_THROW( controller->computeVelocityCommands( pose, velocity, &checker ), std::runtime_error );
// Set with a path -- should attempt to find a sampled point but throw exception
// because it cannot be found, then go to RPP and throw exception because it cannot be transformed
controller->setPlan( path );
EXPECT_THROW( controller->computeVelocityCommands( pose, velocity, &checker ), std::runtime_error );
path.header.frame_id = "base_link";
path.poses[1].pose.position.x = 0.1;
path.poses[1].pose.position.y = 0.1;
path.poses[2].pose.position.x = -1.0;
path.poses[2].pose.position.y = -1.0;
path.poses[2].header.frame_id = "base_link";
path.poses[3].pose.position.x = 10.0;
path.poses[3].pose.position.y = 10.0;
// this should allow it to find the sampled point, then transform to base_link
// validly because we setup the TF for it. The -1.0 should be selected since default min
// is 0.5 and that should cause a rotation in place
controller->setPlan( path );
tf_broadcaster->sendTransform( transform );
auto effort = controller->computeVelocityCommands( pose, velocity, &checker );
EXPECT_EQ( fabs( effort.twist.angular.z ), 1.8 );
path.header.frame_id = "base_link";
path.poses[1].pose.position.x = 0.1;
path.poses[1].pose.position.y = 0.1;
path.poses[2].pose.position.x = 1.0;
path.poses[2].pose.position.y = 0.0;
path.poses[2].header.frame_id = "base_link";
path.poses[3].pose.position.x = 10.0;
path.poses[3].pose.position.y = 10.0;
// this should allow it to find the sampled point, then transform to base_link
// validly because we setup the TF for it. The 1.0 should be selected since default min
// is 0.5 and that should cause a pass off to the RPP controller which will throw
// and exception because the costmap is bogus
controller->setPlan( path );
tf_broadcaster->sendTransform( transform );
EXPECT_THROW( controller->computeVelocityCommands( pose, velocity, &checker ), std::runtime_error );
}
312 TEST( RotationShimControllerTest, testDynamicParameter )
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>( "ShimControllerTest" );
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>( "global_costmap" );
std::string name = "test";
auto tf = std::make_shared<tf2_ros::Buffer>( node->get_clock( ) );
rclcpp_lifecycle::State state;
costmap->on_configure( state );
// set a valid primary controller so we can do lifecycle
node->declare_parameter(
"test.primary_controller",
std::string( "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" ) );
auto controller = std::make_shared<RotationShimShim>( );
controller->configure( node, name, tf, costmap );
controller->activate( );
auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
node->get_node_base_interface( ), node->get_node_topics_interface( ),
node->get_node_graph_interface( ),
node->get_node_services_interface( ) );
auto results = rec_param->set_parameters_atomically(
{rclcpp::Parameter( "test.angular_dist_threshold", 7.0 ),
rclcpp::Parameter( "test.forward_sampling_distance", 7.0 ),
rclcpp::Parameter( "test.rotate_to_heading_angular_vel", 7.0 ),
rclcpp::Parameter( "test.max_angular_accel", 7.0 ),
rclcpp::Parameter( "test.simulate_ahead_time", 7.0 ),
rclcpp::Parameter( "test.primary_controller", std::string( "HI" ) )} );
rclcpp::spin_until_future_complete(
node->get_node_base_interface( ),
results );
EXPECT_EQ( node->get_parameter( "test.angular_dist_threshold" ).as_double( ), 7.0 );
EXPECT_EQ( node->get_parameter( "test.forward_sampling_distance" ).as_double( ), 7.0 );
EXPECT_EQ( node->get_parameter( "test.rotate_to_heading_angular_vel" ).as_double( ), 7.0 );
EXPECT_EQ( node->get_parameter( "test.max_angular_accel" ).as_double( ), 7.0 );
EXPECT_EQ( node->get_parameter( "test.simulate_ahead_time" ).as_double( ), 7.0 );
}
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_RVIZ_PLUGINS__GOAL_COMMON_HPP_
#define NAV2_RVIZ_PLUGINS__GOAL_COMMON_HPP_
#include "nav2_rviz_plugins/goal_pose_updater.hpp"
namespace nav2_rviz_plugins
{
extern GoalPoseUpdater GoalUpdater;
} // nav2_rviz_plugins
#endif // NAV2_RVIZ_PLUGINS__GOAL_COMMON_HPP_
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_RVIZ_PLUGINS__GOAL_POSE_UPDATER_HPP_
#define NAV2_RVIZ_PLUGINS__GOAL_POSE_UPDATER_HPP_
#include <QObject>
namespace nav2_rviz_plugins
{
/// Class to set and update goal pose by emitting signal
24 class GoalPoseUpdater : public QObject
{
Q_OBJECT
public:
GoalPoseUpdater( ) {}
~GoalPoseUpdater( ) {}
void setGoal( double x, double y, double theta, QString frame )
{
emit updateGoal( x, y, theta, frame );
}
37 signals:
void updateGoal( double x, double y, double theta, QString frame );
};
} // namespace nav2_rviz_plugins
#endif // NAV2_RVIZ_PLUGINS__GOAL_POSE_UPDATER_HPP_
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_RVIZ_PLUGINS__GOAL_TOOL_HPP_
#define NAV2_RVIZ_PLUGINS__GOAL_TOOL_HPP_
#include <QObject>
#include <memory>
#include "rviz_default_plugins/tools/pose/pose_tool.hpp"
#include "rviz_default_plugins/visibility_control.hpp"
namespace rviz_common
{
28 class DisplayContext;
namespace properties
{
32 class StringProperty;
} // namespace properties
} // namespace rviz_common
36 namespace nav2_rviz_plugins
{
class RVIZ_DEFAULT_PLUGINS_PUBLIC GoalTool : public rviz_default_plugins::tools::PoseTool
{
Q_OBJECT
public:
GoalTool( );
~GoalTool( ) override;
void onInitialize( ) override;
protected:
void onPoseSet( double x, double y, double theta ) override;
};
} // namespace nav2_rviz_plugins
#endif // NAV2_RVIZ_PLUGINS__GOAL_TOOL_HPP_
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_RVIZ_PLUGINS__NAV2_PANEL_HPP_
#define NAV2_RVIZ_PLUGINS__NAV2_PANEL_HPP_
#include <QtWidgets>
#include <QBasicTimer>
#undef NO_ERROR
#include <memory>
#include <string>
#include <vector>
#include "nav2_lifecycle_manager/lifecycle_manager_client.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "nav2_msgs/action/navigate_through_poses.hpp"
#include "nav2_msgs/action/follow_waypoints.hpp"
#include "nav2_rviz_plugins/ros_action_qevent.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rviz_common/panel.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "visualization_msgs/msg/marker_array.hpp"
#include "nav2_util/geometry_utils.hpp"
38 class QPushButton;
namespace nav2_rviz_plugins
{
43 class InitialThread;
/// Panel to interface to the nav2 stack
46 class Nav2Panel : public rviz_common::Panel
{
Q_OBJECT
public:
explicit Nav2Panel( QWidget * parent = 0 );
virtual ~Nav2Panel( );
void onInitialize( ) override;
/// Load and save configuration data
void load( const rviz_common::Config & config ) override;
void save( rviz_common::Config config ) const override;
private Q_SLOTS:
void startThread( );
void onStartup( );
void onShutdown( );
void onCancel( );
void onPause( );
void onResume( );
void onAccumulatedWp( );
void onAccumulatedNTP( );
void onAccumulating( );
void onNewGoal( double x, double y, double theta, QString frame );
private:
void loadLogFiles( );
void onCancelButtonPressed( );
void timerEvent( QTimerEvent * event ) override;
int unique_id {0};
// Call to send NavigateToPose action request for goal poses
80 void startWaypointFollowing( std::vector<geometry_msgs::msg::PoseStamped> poses );
81 void startNavigation( geometry_msgs::msg::PoseStamped );
82 void startNavThroughPoses( std::vector<geometry_msgs::msg::PoseStamped> poses );
using NavigationGoalHandle =
rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose>;
using WaypointFollowerGoalHandle =
rclcpp_action::ClientGoalHandle<nav2_msgs::action::FollowWaypoints>;
using NavThroughPosesGoalHandle =
rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateThroughPoses>;
// The ( non-spinning ) client node used to invoke the action client
rclcpp::Node::SharedPtr client_node_;
// Timeout value when waiting for action servers to respnd
std::chrono::milliseconds server_timeout_;
// A timer used to check on the completion status of the action
QBasicTimer timer_;
// The NavigateToPose action client
rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SharedPtr navigation_action_client_;
rclcpp_action::Client<nav2_msgs::action::FollowWaypoints>::SharedPtr
waypoint_follower_action_client_;
rclcpp_action::Client<nav2_msgs::action::NavigateThroughPoses>::SharedPtr
nav_through_poses_action_client_;
// Navigation action feedback subscribers
rclcpp::Subscription<nav2_msgs::action::NavigateToPose::Impl::FeedbackMessage>::SharedPtr
navigation_feedback_sub_;
rclcpp::Subscription<nav2_msgs::action::NavigateThroughPoses::Impl::FeedbackMessage>::SharedPtr
nav_through_poses_feedback_sub_;
rclcpp::Subscription<nav2_msgs::action::NavigateToPose::Impl::GoalStatusMessage>::SharedPtr
navigation_goal_status_sub_;
rclcpp::Subscription<nav2_msgs::action::NavigateThroughPoses::Impl::GoalStatusMessage>::SharedPtr
nav_through_poses_goal_status_sub_;
// Goal-related state
nav2_msgs::action::NavigateToPose::Goal navigation_goal_;
nav2_msgs::action::FollowWaypoints::Goal waypoint_follower_goal_;
nav2_msgs::action::NavigateThroughPoses::Goal nav_through_poses_goal_;
NavigationGoalHandle::SharedPtr navigation_goal_handle_;
WaypointFollowerGoalHandle::SharedPtr waypoint_follower_goal_handle_;
NavThroughPosesGoalHandle::SharedPtr nav_through_poses_goal_handle_;
// The client used to control the nav2 stack
std::shared_ptr<nav2_lifecycle_manager::LifecycleManagerClient> client_nav_;
std::shared_ptr<nav2_lifecycle_manager::LifecycleManagerClient> client_loc_;
128 QPushButton * start_reset_button_{nullptr};
129 QPushButton * pause_resume_button_{nullptr};
QPushButton * navigation_mode_button_{nullptr};
QLabel * navigation_status_indicator_{nullptr};
QLabel * localization_status_indicator_{nullptr};
QLabel * navigation_goal_status_indicator_{nullptr};
QLabel * navigation_feedback_indicator_{nullptr};
QStateMachine state_machine_;
InitialThread * initial_thread_;
QState * pre_initial_{nullptr};
QState * initial_{nullptr};
QState * idle_{nullptr};
QState * reset_{nullptr};
QState * paused_{nullptr};
QState * resumed_{nullptr};
// The following states are added to allow for the state of the button to only expose reset
// while the NavigateToPoses action is not active. While running, the user will be allowed to
// cancel the action. The ROSActionTransition allows for the state of the action to be detected
// and the button state to change automatically.
QState * running_{nullptr};
QState * canceled_{nullptr};
// The following states are added to allow to collect several poses to perform a waypoint-mode
// navigation or navigate through poses mode.
QState * accumulating_{nullptr};
QState * accumulated_wp_{nullptr};
QState * accumulated_nav_through_poses_{nullptr};
std::vector<geometry_msgs::msg::PoseStamped> acummulated_poses_;
// Publish the visual markers with the waypoints
void updateWpNavigationMarkers( );
// Create unique id numbers for markers
int getUniqueId( );
void resetUniqueId( );
// create label string from goal status msg
static inline QString getGoalStatusLabel(
int8_t status = action_msgs::msg::GoalStatus::STATUS_UNKNOWN );
// create label string from feedback msg
static inline QString getNavToPoseFeedbackLabel(
nav2_msgs::action::NavigateToPose::Feedback msg =
nav2_msgs::action::NavigateToPose::Feedback( ) );
static inline QString getNavThroughPosesFeedbackLabel(
nav2_msgs::action::NavigateThroughPoses::Feedback =
nav2_msgs::action::NavigateThroughPoses::Feedback( ) );
template<typename T>
static inline std::string toLabel( T & msg );
// round off double to the specified precision and convert to string
static inline std::string toString( double val, int precision = 0 );
// Waypoint navigation visual markers publisher
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr wp_navigation_markers_pub_;
};
class InitialThread : public QThread
{
Q_OBJECT
public:
using SystemStatus = nav2_lifecycle_manager::SystemStatus;
explicit InitialThread(
std::shared_ptr<nav2_lifecycle_manager::LifecycleManagerClient> & client_nav,
std::shared_ptr<nav2_lifecycle_manager::LifecycleManagerClient> & client_loc )
: client_nav_( client_nav ), client_loc_( client_loc )
{}
void run( ) override
{
SystemStatus status_nav = SystemStatus::TIMEOUT;
SystemStatus status_loc = SystemStatus::TIMEOUT;
while ( status_nav == SystemStatus::TIMEOUT ) {
if ( status_nav == SystemStatus::TIMEOUT ) {
status_nav = client_nav_->is_active( std::chrono::seconds( 1 ) );
}
}
// try to communicate twice, might not actually be up if in SLAM mode
bool tried_loc_bringup_once = false;
while ( status_loc == SystemStatus::TIMEOUT ) {
status_loc = client_loc_->is_active( std::chrono::seconds( 1 ) );
if ( tried_loc_bringup_once ) {
break;
}
tried_loc_bringup_once = true;
}
if ( status_nav == SystemStatus::ACTIVE ) {
emit navigationActive( );
} else {
emit navigationInactive( );
}
if ( status_loc == SystemStatus::ACTIVE ) {
emit localizationActive( );
} else {
emit localizationInactive( );
}
}
signals:
void navigationActive( );
void navigationInactive( );
void localizationActive( );
void localizationInactive( );
private:
std::shared_ptr<nav2_lifecycle_manager::LifecycleManagerClient> client_nav_;
std::shared_ptr<nav2_lifecycle_manager::LifecycleManagerClient> client_loc_;
};
} // namespace nav2_rviz_plugins
#endif // NAV2_RVIZ_PLUGINS__NAV2_PANEL_HPP_
1 /*
* Copyright ( c ) 2012, Willow Garage, Inc.
* Copyright ( c ) 2018, Bosch Software Innovations GmbH.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES ( INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE )
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
// Copyright ( c ) 2019 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_
#define NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_
#include <vector>
#include <OgreManualObject.h>
#include <OgreMaterialManager.h>
#include <OgreSceneNode.h>
#include <OgreVector3.h>
#include <OgreQuaternion.h>
#include "nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp"
namespace nav2_rviz_plugins
{
struct OgrePoseWithWeight;
64 class FlatWeightedArrowsArray
{
public:
67 explicit FlatWeightedArrowsArray( Ogre::SceneManager * scene_manager_ );
68 ~FlatWeightedArrowsArray( );
70 void createAndAttachManualObject( Ogre::SceneNode * scene_node );
71 void updateManualObject(
72 Ogre::ColourValue color,
float alpha,
float min_length,
float max_length,
76 const std::vector<nav2_rviz_plugins::OgrePoseWithWeight> & poses );
77 void clear( );
private:
80 void setManualObjectMaterial( );
81 void setManualObjectVertices(
82 const Ogre::ColourValue & color,
float min_length,
float max_length,
85 const std::vector<nav2_rviz_plugins::OgrePoseWithWeight> & poses );
87 Ogre::SceneManager * scene_manager_;
88 Ogre::ManualObject * manual_object_;
89 Ogre::MaterialPtr material_;
};
} // namespace nav2_rviz_plugins
#endif // NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__FLAT_WEIGHTED_ARROWS_ARRAY_HPP_
/*
* Copyright ( c ) 2012, Willow Garage, Inc.
* Copyright ( c ) 2018, Bosch Software Innovations GmbH.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES ( INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE )
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
// Copyright ( c ) 2019 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__PARTICLE_CLOUD_DISPLAY_HPP_
#define NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__PARTICLE_CLOUD_DISPLAY_HPP_
#include <memory>
#include <vector>
#include "nav2_msgs/msg/particle_cloud.hpp"
#include "rviz_rendering/objects/shape.hpp"
#include "rviz_common/message_filter_display.hpp"
namespace Ogre
{
59 class ManualObject;
} // namespace Ogre
namespace rviz_common
{
namespace properties
{
66 class EnumProperty;
67 class ColorProperty;
68 class FloatProperty;
} // namespace properties
} // namespace rviz_common
namespace rviz_rendering
{
74 class Arrow;
75 class Axes;
} // namespace rviz_rendering
namespace nav2_rviz_plugins
{
80 class FlatWeightedArrowsArray;
struct OgrePoseWithWeight
{
Ogre::Vector3 position;
Ogre::Quaternion orientation;
float weight;
};
/** @brief Displays a nav2_msgs/ParticleCloud message as a bunch of line-drawn weighted arrows. */
89 class ParticleCloudDisplay : public rviz_common::MessageFilterDisplay<nav2_msgs::msg::ParticleCloud>
{
Q_OBJECT
public:
// TODO( botteroa-si ): Constructor for testing, remove once ros_nodes can be mocked and call
// initialize instead
ParticleCloudDisplay(
rviz_common::DisplayContext * display_context,
Ogre::SceneNode * scene_node );
99 ParticleCloudDisplay( );
100 ~ParticleCloudDisplay( ) override;
102 void processMessage( nav2_msgs::msg::ParticleCloud::ConstSharedPtr msg ) override;
103 void setShape( QString shape ); // for testing
protected:
106 void onInitialize( ) override;
107 void reset( ) override;
109 private Q_SLOTS:
/// Update the interface and visible shapes based on the selected shape type.
void updateShapeChoice( );
/// Update the arrow color.
114 void updateArrowColor( );
/// Update arrow geometry
117 void updateGeometry( );
private:
void initializeProperties( );
bool validateFloats( const nav2_msgs::msg::ParticleCloud & msg );
bool setTransform( std_msgs::msg::Header const & header );
void updateDisplay( );
void updateArrows2d( );
void updateArrows3d( );
void updateAxes( );
void updateArrow3dGeometry( );
void updateAxesGeometry( );
std::unique_ptr<rviz_rendering::Axes> makeAxes( );
std::unique_ptr<rviz_rendering::Arrow> makeArrow3d( );
std::vector<OgrePoseWithWeight> poses_;
std::unique_ptr<FlatWeightedArrowsArray> arrows2d_;
std::vector<std::unique_ptr<rviz_rendering::Arrow>> arrows3d_;
std::vector<std::unique_ptr<rviz_rendering::Axes>> axes_;
Ogre::SceneNode * arrow_node_;
Ogre::SceneNode * axes_node_;
rviz_common::properties::EnumProperty * shape_property_;
rviz_common::properties::ColorProperty * arrow_color_property_;
rviz_common::properties::FloatProperty * arrow_alpha_property_;
rviz_common::properties::FloatProperty * arrow_min_length_property_;
rviz_common::properties::FloatProperty * arrow_max_length_property_;
float min_length_;
float max_length_;
float length_scale_;
float head_radius_scale_;
float head_length_scale_;
float shaft_radius_scale_;
};
} // namespace nav2_rviz_plugins
#endif // NAV2_RVIZ_PLUGINS__PARTICLE_CLOUD_DISPLAY__PARTICLE_CLOUD_DISPLAY_HPP_
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_RVIZ_PLUGINS__ROS_ACTION_QEVENT_HPP_
#define NAV2_RVIZ_PLUGINS__ROS_ACTION_QEVENT_HPP_
#include <QAbstractTransition>
namespace nav2_rviz_plugins
{
23 enum class QActionState
{
ACTIVE,
INACTIVE
};
/// Custom Event to track state of ROS Action
struct ROSActionQEvent : public QEvent
{
explicit ROSActionQEvent( QActionState state )
: QEvent( QEvent::Type( QEvent::User + 1 ) ),
state_( state ) {}
QActionState state_;
};
/// Custom Transition to check whether ROS Action state has changed
40 class ROSActionQTransition : public QAbstractTransition
{
public:
43 explicit ROSActionQTransition( QActionState initial_status )
: status_( initial_status )
{}
47 ~ROSActionQTransition( ) {}
protected:
50 virtual bool eventTest( QEvent * e )
{
if ( e->type( ) != QEvent::Type( QEvent::User + 1 ) ) { // ROSActionEvent
return false;
}
ROSActionQEvent * action_event = static_cast<ROSActionQEvent *>( e );
return status_ != action_event->state_;
}
59 virtual void onTransition( QEvent * ) {}
60 QActionState status_;
};
} // namespace nav2_rviz_plugins
#endif // NAV2_RVIZ_PLUGINS__ROS_ACTION_QEVENT_HPP_
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "nav2_rviz_plugins/goal_tool.hpp"
#include <memory>
#include <string>
#include "nav2_rviz_plugins/goal_common.hpp"
#include "rviz_common/display_context.hpp"
#include "rviz_common/load_resource.hpp"
namespace nav2_rviz_plugins
{
27 GoalTool::GoalTool( )
: rviz_default_plugins::tools::PoseTool( )
{
shortcut_key_ = 'g';
}
33 GoalTool::~GoalTool( )
{
}
37 void GoalTool::onInitialize( )
{
PoseTool::onInitialize( );
setName( "Nav2 Goal" );
setIcon( rviz_common::loadPixmap( "package://rviz_default_plugins/icons/classes/SetGoal.png" ) );
}
void
45 GoalTool::onPoseSet( double x, double y, double theta )
{
// Set goal pose on global object GoalUpdater to update nav2 Panel
GoalUpdater.setGoal( x, y, theta, context_->getFixedFrame( ) );
}
} // namespace nav2_rviz_plugins
#include <pluginlib/class_list_macros.hpp> // NOLINT
54 PLUGINLIB_EXPORT_CLASS( nav2_rviz_plugins::GoalTool, rviz_common::Tool )
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "nav2_rviz_plugins/nav2_panel.hpp"
#include <QtConcurrent/QtConcurrent>
#include <QVBoxLayout>
#include <memory>
#include <vector>
#include <utility>
#include <chrono>
#include <string>
#include "nav2_rviz_plugins/goal_common.hpp"
#include "rviz_common/display_context.hpp"
#include "ament_index_cpp/get_package_share_directory.hpp"
using namespace std::chrono_literals;
namespace nav2_rviz_plugins
{
using nav2_util::geometry_utils::orientationAroundZAxis;
// Define global GoalPoseUpdater so that the nav2 GoalTool plugin can access to update goal pose
GoalPoseUpdater GoalUpdater;
39 Nav2Panel::Nav2Panel( QWidget * parent )
: Panel( parent ),
server_timeout_( 100 )
{
// Create the control button and its tooltip
start_reset_button_ = new QPushButton;
pause_resume_button_ = new QPushButton;
navigation_mode_button_ = new QPushButton;
navigation_status_indicator_ = new QLabel;
localization_status_indicator_ = new QLabel;
navigation_goal_status_indicator_ = new QLabel;
navigation_feedback_indicator_ = new QLabel;
// Create the state machine used to present the proper control button states in the UI
const char * startup_msg = "Configure and activate all nav2 lifecycle nodes";
const char * shutdown_msg = "Deactivate and cleanup all nav2 lifecycle nodes";
const char * cancel_msg = "Cancel navigation";
const char * pause_msg = "Deactivate all nav2 lifecycle nodes";
const char * resume_msg = "Activate all nav2 lifecycle nodes";
const char * single_goal_msg = "Change to waypoint / nav through poses style navigation";
const char * waypoint_goal_msg = "Start following waypoints";
const char * nft_goal_msg = "Start navigating through poses";
const char * cancel_waypoint_msg = "Cancel waypoint / viapoint accumulation mode";
const QString navigation_active( "<table><tr><td width=100><b>Navigation:</b></td>"
"<td><font color=green>active</color></td></tr></table>" );
const QString navigation_inactive( "<table><tr><td width=100><b>Navigation:</b></td>"
"<td>inactive</td></tr></table>" );
const QString navigation_unknown( "<table><tr><td width=100><b>Navigation:</b></td>"
"<td>unknown</td></tr></table>" );
const QString localization_active( "<table><tr><td width=100><b>Localization:</b></td>"
"<td><font color=green>active</color></td></tr></table>" );
const QString localization_inactive( "<table><tr><td width=100><b>Localization:</b></td>"
"<td>inactive</td></tr></table>" );
const QString localization_unknown( "<table><tr><td width=100><b>Localization:</b></td>"
"<td>unknown</td></tr></table>" );
navigation_status_indicator_->setText( navigation_unknown );
localization_status_indicator_->setText( localization_unknown );
navigation_goal_status_indicator_->setText( getGoalStatusLabel( ) );
navigation_feedback_indicator_->setText( getNavThroughPosesFeedbackLabel( ) );
navigation_status_indicator_->setSizePolicy( QSizePolicy::Fixed, QSizePolicy::Fixed );
localization_status_indicator_->setSizePolicy( QSizePolicy::Fixed, QSizePolicy::Fixed );
navigation_goal_status_indicator_->setSizePolicy( QSizePolicy::Fixed, QSizePolicy::Fixed );
navigation_feedback_indicator_->setSizePolicy( QSizePolicy::Fixed, QSizePolicy::Fixed );
pre_initial_ = new QState( );
pre_initial_->setObjectName( "pre_initial" );
pre_initial_->assignProperty( start_reset_button_, "text", "Startup" );
pre_initial_->assignProperty( start_reset_button_, "enabled", false );
pre_initial_->assignProperty( pause_resume_button_, "text", "Pause" );
pre_initial_->assignProperty( pause_resume_button_, "enabled", false );
pre_initial_->assignProperty(
navigation_mode_button_, "text",
"Waypoint / Nav Through Poses Mode" );
pre_initial_->assignProperty( navigation_mode_button_, "enabled", false );
initial_ = new QState( );
initial_->setObjectName( "initial" );
initial_->assignProperty( start_reset_button_, "text", "Startup" );
initial_->assignProperty( start_reset_button_, "toolTip", startup_msg );
initial_->assignProperty( start_reset_button_, "enabled", true );
initial_->assignProperty( pause_resume_button_, "text", "Pause" );
initial_->assignProperty( pause_resume_button_, "enabled", false );
initial_->assignProperty( navigation_mode_button_, "text", "Waypoint / Nav Through Poses Mode" );
initial_->assignProperty( navigation_mode_button_, "enabled", false );
// State entered when navigate_to_pose action is not active
idle_ = new QState( );
idle_->setObjectName( "idle" );
idle_->assignProperty( start_reset_button_, "text", "Reset" );
idle_->assignProperty( start_reset_button_, "toolTip", shutdown_msg );
idle_->assignProperty( start_reset_button_, "enabled", true );
idle_->assignProperty( pause_resume_button_, "text", "Pause" );
idle_->assignProperty( pause_resume_button_, "enabled", true );
idle_->assignProperty( pause_resume_button_, "toolTip", pause_msg );
idle_->assignProperty( navigation_mode_button_, "text", "Waypoint / Nav Through Poses Mode" );
idle_->assignProperty( navigation_mode_button_, "enabled", true );
idle_->assignProperty( navigation_mode_button_, "toolTip", single_goal_msg );
// State entered when navigate_to_pose action is not active
accumulating_ = new QState( );
accumulating_->setObjectName( "accumulating" );
accumulating_->assignProperty( start_reset_button_, "text", "Cancel Accumulation" );
accumulating_->assignProperty( start_reset_button_, "toolTip", cancel_waypoint_msg );
accumulating_->assignProperty( start_reset_button_, "enabled", true );
accumulating_->assignProperty( pause_resume_button_, "text", "Start Nav Through Poses" );
accumulating_->assignProperty( pause_resume_button_, "enabled", true );
accumulating_->assignProperty( pause_resume_button_, "toolTip", nft_goal_msg );
accumulating_->assignProperty( navigation_mode_button_, "text", "Start Waypoint Following" );
accumulating_->assignProperty( navigation_mode_button_, "enabled", true );
accumulating_->assignProperty( navigation_mode_button_, "toolTip", waypoint_goal_msg );
accumulated_wp_ = new QState( );
accumulated_wp_->setObjectName( "accumulated_wp" );
accumulated_wp_->assignProperty( start_reset_button_, "text", "Cancel" );
accumulated_wp_->assignProperty( start_reset_button_, "toolTip", cancel_msg );
accumulated_wp_->assignProperty( start_reset_button_, "enabled", true );
accumulated_wp_->assignProperty( pause_resume_button_, "text", "Start Nav Through Poses" );
accumulated_wp_->assignProperty( pause_resume_button_, "enabled", false );
accumulated_wp_->assignProperty( pause_resume_button_, "toolTip", nft_goal_msg );
accumulated_wp_->assignProperty( navigation_mode_button_, "text", "Start Waypoint Following" );
accumulated_wp_->assignProperty( navigation_mode_button_, "enabled", false );
accumulated_wp_->assignProperty( navigation_mode_button_, "toolTip", waypoint_goal_msg );
accumulated_nav_through_poses_ = new QState( );
accumulated_nav_through_poses_->setObjectName( "accumulated_nav_through_poses" );
accumulated_nav_through_poses_->assignProperty( start_reset_button_, "text", "Cancel" );
accumulated_nav_through_poses_->assignProperty( start_reset_button_, "toolTip", cancel_msg );
accumulated_nav_through_poses_->assignProperty( start_reset_button_, "enabled", true );
accumulated_nav_through_poses_->assignProperty(
pause_resume_button_, "text",
"Start Nav Through Poses" );
accumulated_nav_through_poses_->assignProperty( pause_resume_button_, "enabled", false );
accumulated_nav_through_poses_->assignProperty( pause_resume_button_, "toolTip", nft_goal_msg );
accumulated_nav_through_poses_->assignProperty(
navigation_mode_button_, "text",
"Start Waypoint Following" );
accumulated_nav_through_poses_->assignProperty( navigation_mode_button_, "enabled", false );
accumulated_nav_through_poses_->assignProperty(
navigation_mode_button_, "toolTip",
waypoint_goal_msg );
// State entered to cancel the navigate_to_pose action
canceled_ = new QState( );
canceled_->setObjectName( "canceled" );
// State entered to reset the nav2 lifecycle nodes
reset_ = new QState( );
reset_->setObjectName( "reset" );
// State entered while the navigate_to_pose action is active
running_ = new QState( );
running_->setObjectName( "running" );
running_->assignProperty( start_reset_button_, "text", "Cancel" );
running_->assignProperty( start_reset_button_, "toolTip", cancel_msg );
running_->assignProperty( pause_resume_button_, "text", "Pause" );
running_->assignProperty( pause_resume_button_, "enabled", false );
running_->assignProperty( navigation_mode_button_, "text", "Waypoint mode" );
running_->assignProperty( navigation_mode_button_, "enabled", false );
// State entered when pause is requested
paused_ = new QState( );
paused_->setObjectName( "pausing" );
paused_->assignProperty( start_reset_button_, "text", "Reset" );
paused_->assignProperty( start_reset_button_, "toolTip", shutdown_msg );
paused_->assignProperty( pause_resume_button_, "text", "Resume" );
paused_->assignProperty( pause_resume_button_, "toolTip", resume_msg );
paused_->assignProperty( pause_resume_button_, "enabled", true );
paused_->assignProperty( navigation_mode_button_, "text", "Start navigation" );
paused_->assignProperty( navigation_mode_button_, "toolTip", resume_msg );
paused_->assignProperty( navigation_mode_button_, "enabled", true );
// State entered to resume the nav2 lifecycle nodes
resumed_ = new QState( );
resumed_->setObjectName( "resuming" );
QObject::connect( initial_, SIGNAL( exited( ) ), this, SLOT( onStartup( ) ) );
QObject::connect( canceled_, SIGNAL( exited( ) ), this, SLOT( onCancel( ) ) );
QObject::connect( reset_, SIGNAL( exited( ) ), this, SLOT( onShutdown( ) ) );
QObject::connect( paused_, SIGNAL( entered( ) ), this, SLOT( onPause( ) ) );
QObject::connect( resumed_, SIGNAL( exited( ) ), this, SLOT( onResume( ) ) );
QObject::connect( accumulating_, SIGNAL( entered( ) ), this, SLOT( onAccumulating( ) ) );
QObject::connect( accumulated_wp_, SIGNAL( entered( ) ), this, SLOT( onAccumulatedWp( ) ) );
QObject::connect(
accumulated_nav_through_poses_, SIGNAL( entered( ) ), this,
SLOT( onAccumulatedNTP( ) ) );
// Start/Reset button click transitions
initial_->addTransition( start_reset_button_, SIGNAL( clicked( ) ), idle_ );
idle_->addTransition( start_reset_button_, SIGNAL( clicked( ) ), reset_ );
running_->addTransition( start_reset_button_, SIGNAL( clicked( ) ), canceled_ );
paused_->addTransition( start_reset_button_, SIGNAL( clicked( ) ), reset_ );
idle_->addTransition( navigation_mode_button_, SIGNAL( clicked( ) ), accumulating_ );
accumulating_->addTransition( navigation_mode_button_, SIGNAL( clicked( ) ), accumulated_wp_ );
accumulating_->addTransition(
pause_resume_button_, SIGNAL(
clicked( ) ), accumulated_nav_through_poses_ );
accumulating_->addTransition( start_reset_button_, SIGNAL( clicked( ) ), idle_ );
accumulated_wp_->addTransition( start_reset_button_, SIGNAL( clicked( ) ), canceled_ );
accumulated_nav_through_poses_->addTransition( start_reset_button_, SIGNAL( clicked( ) ), canceled_ );
// Internal state transitions
canceled_->addTransition( canceled_, SIGNAL( entered( ) ), idle_ );
reset_->addTransition( reset_, SIGNAL( entered( ) ), initial_ );
resumed_->addTransition( resumed_, SIGNAL( entered( ) ), idle_ );
// Pause/Resume button click transitions
idle_->addTransition( pause_resume_button_, SIGNAL( clicked( ) ), paused_ );
paused_->addTransition( pause_resume_button_, SIGNAL( clicked( ) ), resumed_ );
// ROSAction Transitions: So when actions are updated remotely ( failing, succeeding, etc )
// the state of the application will also update. This means that if in the processing
// states and then goes inactive, move back to the idle state. Vise versa as well.
ROSActionQTransition * idleTransition = new ROSActionQTransition( QActionState::INACTIVE );
idleTransition->setTargetState( running_ );
idle_->addTransition( idleTransition );
ROSActionQTransition * runningTransition = new ROSActionQTransition( QActionState::ACTIVE );
runningTransition->setTargetState( idle_ );
running_->addTransition( runningTransition );
ROSActionQTransition * idleAccumulatedWpTransition =
new ROSActionQTransition( QActionState::INACTIVE );
idleAccumulatedWpTransition->setTargetState( accumulated_wp_ );
idle_->addTransition( idleAccumulatedWpTransition );
ROSActionQTransition * accumulatedWpTransition = new ROSActionQTransition( QActionState::ACTIVE );
accumulatedWpTransition->setTargetState( idle_ );
accumulated_wp_->addTransition( accumulatedWpTransition );
ROSActionQTransition * idleAccumulatedNTPTransition =
new ROSActionQTransition( QActionState::INACTIVE );
idleAccumulatedNTPTransition->setTargetState( accumulated_nav_through_poses_ );
idle_->addTransition( idleAccumulatedNTPTransition );
ROSActionQTransition * accumulatedNTPTransition = new ROSActionQTransition( QActionState::ACTIVE );
accumulatedNTPTransition->setTargetState( idle_ );
accumulated_nav_through_poses_->addTransition( accumulatedNTPTransition );
auto options = rclcpp::NodeOptions( ).arguments(
{"--ros-args --remap __node:=navigation_dialog_action_client"} );
client_node_ = std::make_shared<rclcpp::Node>( "_", options );
client_nav_ = std::make_shared<nav2_lifecycle_manager::LifecycleManagerClient>(
"lifecycle_manager_navigation", client_node_ );
client_loc_ = std::make_shared<nav2_lifecycle_manager::LifecycleManagerClient>(
"lifecycle_manager_localization", client_node_ );
initial_thread_ = new InitialThread( client_nav_, client_loc_ );
connect( initial_thread_, &InitialThread::finished, initial_thread_, &QObject::deleteLater );
QSignalTransition * activeSignal = new QSignalTransition(
initial_thread_,
&InitialThread::navigationActive );
activeSignal->setTargetState( idle_ );
pre_initial_->addTransition( activeSignal );
QSignalTransition * inactiveSignal = new QSignalTransition(
initial_thread_,
&InitialThread::navigationInactive );
inactiveSignal->setTargetState( initial_ );
pre_initial_->addTransition( inactiveSignal );
QObject::connect(
initial_thread_, &InitialThread::navigationActive,
[this, navigation_active] {
navigation_status_indicator_->setText( navigation_active );
} );
QObject::connect(
initial_thread_, &InitialThread::navigationInactive,
[this, navigation_inactive] {
navigation_status_indicator_->setText( navigation_inactive );
navigation_goal_status_indicator_->setText( getGoalStatusLabel( ) );
navigation_feedback_indicator_->setText( getNavThroughPosesFeedbackLabel( ) );
} );
QObject::connect(
initial_thread_, &InitialThread::localizationActive,
[this, localization_active] {
localization_status_indicator_->setText( localization_active );
} );
QObject::connect(
initial_thread_, &InitialThread::localizationInactive,
[this, localization_inactive] {
localization_status_indicator_->setText( localization_inactive );
} );
state_machine_.addState( pre_initial_ );
state_machine_.addState( initial_ );
state_machine_.addState( idle_ );
state_machine_.addState( running_ );
state_machine_.addState( canceled_ );
state_machine_.addState( reset_ );
state_machine_.addState( paused_ );
state_machine_.addState( resumed_ );
state_machine_.addState( accumulating_ );
state_machine_.addState( accumulated_wp_ );
state_machine_.addState( accumulated_nav_through_poses_ );
state_machine_.setInitialState( pre_initial_ );
// delay starting initial thread until state machine has started or a race occurs
QObject::connect( &state_machine_, SIGNAL( started( ) ), this, SLOT( startThread( ) ) );
state_machine_.start( );
// Lay out the items in the panel
QVBoxLayout * main_layout = new QVBoxLayout;
main_layout->addWidget( navigation_status_indicator_ );
main_layout->addWidget( localization_status_indicator_ );
main_layout->addWidget( navigation_goal_status_indicator_ );
main_layout->addWidget( navigation_feedback_indicator_ );
main_layout->addWidget( pause_resume_button_ );
main_layout->addWidget( start_reset_button_ );
main_layout->addWidget( navigation_mode_button_ );
main_layout->setContentsMargins( 10, 10, 10, 10 );
setLayout( main_layout );
navigation_action_client_ =
rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(
client_node_,
"navigate_to_pose" );
waypoint_follower_action_client_ =
rclcpp_action::create_client<nav2_msgs::action::FollowWaypoints>(
client_node_,
"follow_waypoints" );
nav_through_poses_action_client_ =
rclcpp_action::create_client<nav2_msgs::action::NavigateThroughPoses>(
client_node_,
"navigate_through_poses" );
navigation_goal_ = nav2_msgs::action::NavigateToPose::Goal( );
waypoint_follower_goal_ = nav2_msgs::action::FollowWaypoints::Goal( );
nav_through_poses_goal_ = nav2_msgs::action::NavigateThroughPoses::Goal( );
wp_navigation_markers_pub_ =
client_node_->create_publisher<visualization_msgs::msg::MarkerArray>(
"waypoints",
rclcpp::QoS( 1 ).transient_local( ) );
QObject::connect(
&GoalUpdater, SIGNAL( updateGoal( double, double, double, QString ) ), // NOLINT
this, SLOT( onNewGoal( double, double, double, QString ) ) ); // NOLINT
}
380 Nav2Panel::~Nav2Panel( )
{
}
void
385 Nav2Panel::onInitialize( )
{
auto node = getDisplayContext( )->getRosNodeAbstraction( ).lock( )->get_raw_node( );
// create action feedback subscribers
navigation_feedback_sub_ =
node->create_subscription<nav2_msgs::action::NavigateToPose::Impl::FeedbackMessage>(
"navigate_to_pose/_action/feedback",
rclcpp::SystemDefaultsQoS( ),
[this]( const nav2_msgs::action::NavigateToPose::Impl::FeedbackMessage::SharedPtr msg ) {
navigation_feedback_indicator_->setText( getNavToPoseFeedbackLabel( msg->feedback ) );
} );
nav_through_poses_feedback_sub_ =
node->create_subscription<nav2_msgs::action::NavigateThroughPoses::Impl::FeedbackMessage>(
"navigate_through_poses/_action/feedback",
rclcpp::SystemDefaultsQoS( ),
[this]( const nav2_msgs::action::NavigateThroughPoses::Impl::FeedbackMessage::SharedPtr msg ) {
navigation_feedback_indicator_->setText( getNavThroughPosesFeedbackLabel( msg->feedback ) );
} );
// create action goal status subscribers
navigation_goal_status_sub_ = node->create_subscription<action_msgs::msg::GoalStatusArray>(
"navigate_to_pose/_action/status",
rclcpp::SystemDefaultsQoS( ),
[this]( const action_msgs::msg::GoalStatusArray::SharedPtr msg ) {
navigation_goal_status_indicator_->setText(
getGoalStatusLabel( msg->status_list.back( ).status ) );
if ( msg->status_list.back( ).status != action_msgs::msg::GoalStatus::STATUS_EXECUTING ) {
navigation_feedback_indicator_->setText( getNavToPoseFeedbackLabel( ) );
}
} );
nav_through_poses_goal_status_sub_ = node->create_subscription<action_msgs::msg::GoalStatusArray>(
"navigate_through_poses/_action/status",
rclcpp::SystemDefaultsQoS( ),
[this]( const action_msgs::msg::GoalStatusArray::SharedPtr msg ) {
navigation_goal_status_indicator_->setText(
getGoalStatusLabel( msg->status_list.back( ).status ) );
if ( msg->status_list.back( ).status != action_msgs::msg::GoalStatus::STATUS_EXECUTING ) {
navigation_feedback_indicator_->setText( getNavThroughPosesFeedbackLabel( ) );
}
} );
}
void
429 Nav2Panel::startThread( )
{
// start initial thread now that state machine is started
initial_thread_->start( );
}
void
436 Nav2Panel::onPause( )
{
QFuture<void> futureNav =
QtConcurrent::run(
std::bind(
&nav2_lifecycle_manager::LifecycleManagerClient::pause,
client_nav_.get( ), std::placeholders::_1 ), server_timeout_ );
QFuture<void> futureLoc =
QtConcurrent::run(
std::bind(
&nav2_lifecycle_manager::LifecycleManagerClient::pause,
client_loc_.get( ), std::placeholders::_1 ), server_timeout_ );
}
void
451 Nav2Panel::onResume( )
{
QFuture<void> futureNav =
QtConcurrent::run(
std::bind(
&nav2_lifecycle_manager::LifecycleManagerClient::resume,
client_nav_.get( ), std::placeholders::_1 ), server_timeout_ );
QFuture<void> futureLoc =
QtConcurrent::run(
std::bind(
&nav2_lifecycle_manager::LifecycleManagerClient::resume,
client_loc_.get( ), std::placeholders::_1 ), server_timeout_ );
}
void
466 Nav2Panel::onStartup( )
{
QFuture<void> futureNav =
QtConcurrent::run(
std::bind(
&nav2_lifecycle_manager::LifecycleManagerClient::startup,
client_nav_.get( ), std::placeholders::_1 ), server_timeout_ );
QFuture<void> futureLoc =
QtConcurrent::run(
std::bind(
&nav2_lifecycle_manager::LifecycleManagerClient::startup,
client_loc_.get( ), std::placeholders::_1 ), server_timeout_ );
}
void
481 Nav2Panel::onShutdown( )
{
QFuture<void> futureNav =
QtConcurrent::run(
std::bind(
&nav2_lifecycle_manager::LifecycleManagerClient::reset,
client_nav_.get( ), std::placeholders::_1 ), server_timeout_ );
QFuture<void> futureLoc =
QtConcurrent::run(
std::bind(
&nav2_lifecycle_manager::LifecycleManagerClient::reset,
client_loc_.get( ), std::placeholders::_1 ), server_timeout_ );
timer_.stop( );
}
void
497 Nav2Panel::onCancel( )
{
QFuture<void> future =
QtConcurrent::run(
std::bind(
&Nav2Panel::onCancelButtonPressed,
this ) );
}
void
507 Nav2Panel::onNewGoal( double x, double y, double theta, QString frame )
{
auto pose = geometry_msgs::msg::PoseStamped( );
pose.header.stamp = rclcpp::Clock( ).now( );
pose.header.frame_id = frame.toStdString( );
pose.pose.position.x = x;
pose.pose.position.y = y;
pose.pose.position.z = 0.0;
pose.pose.orientation = orientationAroundZAxis( theta );
if ( state_machine_.configuration( ).contains( accumulating_ ) ) {
acummulated_poses_.push_back( pose );
} else {
std::cout << "Start navigation" << std::endl;
startNavigation( pose );
}
updateWpNavigationMarkers( );
}
void
529 Nav2Panel::onCancelButtonPressed( )
{
if ( navigation_goal_handle_ ) {
auto future_cancel = navigation_action_client_->async_cancel_goal( navigation_goal_handle_ );
if ( rclcpp::spin_until_future_complete( client_node_, future_cancel, server_timeout_ ) !=
rclcpp::FutureReturnCode::SUCCESS )
{
RCLCPP_ERROR( client_node_->get_logger( ), "Failed to cancel goal" );
} else {
navigation_goal_handle_.reset( );
}
}
if ( waypoint_follower_goal_handle_ ) {
auto future_cancel =
waypoint_follower_action_client_->async_cancel_goal( waypoint_follower_goal_handle_ );
if ( rclcpp::spin_until_future_complete( client_node_, future_cancel, server_timeout_ ) !=
rclcpp::FutureReturnCode::SUCCESS )
{
RCLCPP_ERROR( client_node_->get_logger( ), "Failed to cancel waypoint follower" );
} else {
waypoint_follower_goal_handle_.reset( );
}
}
if ( nav_through_poses_goal_handle_ ) {
auto future_cancel =
nav_through_poses_action_client_->async_cancel_goal( nav_through_poses_goal_handle_ );
if ( rclcpp::spin_until_future_complete( client_node_, future_cancel, server_timeout_ ) !=
rclcpp::FutureReturnCode::SUCCESS )
{
RCLCPP_ERROR( client_node_->get_logger( ), "Failed to cancel nav through pose action" );
} else {
nav_through_poses_goal_handle_.reset( );
}
}
timer_.stop( );
}
void
574 Nav2Panel::onAccumulatedWp( )
{
std::cout << "Start waypoint" << std::endl;
startWaypointFollowing( acummulated_poses_ );
acummulated_poses_.clear( );
}
void
582 Nav2Panel::onAccumulatedNTP( )
{
std::cout << "Start navigate through poses" << std::endl;
startNavThroughPoses( acummulated_poses_ );
acummulated_poses_.clear( );
}
void
590 Nav2Panel::onAccumulating( )
{
acummulated_poses_.clear( );
}
void
596 Nav2Panel::timerEvent( QTimerEvent * event )
{
if ( state_machine_.configuration( ).contains( accumulated_wp_ ) ) {
if ( event->timerId( ) == timer_.timerId( ) ) {
if ( !waypoint_follower_goal_handle_ ) {
RCLCPP_DEBUG( client_node_->get_logger( ), "Waiting for Goal" );
state_machine_.postEvent( new ROSActionQEvent( QActionState::INACTIVE ) );
return;
}
rclcpp::spin_some( client_node_ );
auto status = waypoint_follower_goal_handle_->get_status( );
// Check if the goal is still executing
if ( status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED ||
status == action_msgs::msg::GoalStatus::STATUS_EXECUTING )
{
state_machine_.postEvent( new ROSActionQEvent( QActionState::ACTIVE ) );
} else {
state_machine_.postEvent( new ROSActionQEvent( QActionState::INACTIVE ) );
timer_.stop( );
}
}
} else if ( state_machine_.configuration( ).contains( accumulated_nav_through_poses_ ) ) {
if ( event->timerId( ) == timer_.timerId( ) ) {
if ( !nav_through_poses_goal_handle_ ) {
RCLCPP_DEBUG( client_node_->get_logger( ), "Waiting for Goal" );
state_machine_.postEvent( new ROSActionQEvent( QActionState::INACTIVE ) );
return;
}
rclcpp::spin_some( client_node_ );
auto status = nav_through_poses_goal_handle_->get_status( );
// Check if the goal is still executing
if ( status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED ||
status == action_msgs::msg::GoalStatus::STATUS_EXECUTING )
{
state_machine_.postEvent( new ROSActionQEvent( QActionState::ACTIVE ) );
} else {
state_machine_.postEvent( new ROSActionQEvent( QActionState::INACTIVE ) );
timer_.stop( );
}
}
} else {
if ( event->timerId( ) == timer_.timerId( ) ) {
if ( !navigation_goal_handle_ ) {
RCLCPP_DEBUG( client_node_->get_logger( ), "Waiting for Goal" );
state_machine_.postEvent( new ROSActionQEvent( QActionState::INACTIVE ) );
return;
}
rclcpp::spin_some( client_node_ );
auto status = navigation_goal_handle_->get_status( );
// Check if the goal is still executing
if ( status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED ||
status == action_msgs::msg::GoalStatus::STATUS_EXECUTING )
{
state_machine_.postEvent( new ROSActionQEvent( QActionState::ACTIVE ) );
} else {
state_machine_.postEvent( new ROSActionQEvent( QActionState::INACTIVE ) );
timer_.stop( );
}
}
}
}
void
665 Nav2Panel::startWaypointFollowing( std::vector<geometry_msgs::msg::PoseStamped> poses )
{
auto is_action_server_ready =
waypoint_follower_action_client_->wait_for_action_server( std::chrono::seconds( 5 ) );
if ( !is_action_server_ready ) {
RCLCPP_ERROR(
client_node_->get_logger( ), "follow_waypoints action server is not available."
" Is the initial pose set?" );
return;
}
// Send the goal poses
waypoint_follower_goal_.poses = poses;
RCLCPP_DEBUG(
client_node_->get_logger( ), "Sending a path of %zu waypoints:",
waypoint_follower_goal_.poses.size( ) );
for ( auto waypoint : waypoint_follower_goal_.poses ) {
RCLCPP_DEBUG(
client_node_->get_logger( ),
"\t( %lf, %lf )", waypoint.pose.position.x, waypoint.pose.position.y );
}
// Enable result awareness by providing an empty lambda function
auto send_goal_options =
rclcpp_action::Client<nav2_msgs::action::FollowWaypoints>::SendGoalOptions( );
send_goal_options.result_callback = [this]( auto ) {
waypoint_follower_goal_handle_.reset( );
};
auto future_goal_handle =
waypoint_follower_action_client_->async_send_goal( waypoint_follower_goal_, send_goal_options );
if ( rclcpp::spin_until_future_complete( client_node_, future_goal_handle, server_timeout_ ) !=
rclcpp::FutureReturnCode::SUCCESS )
{
RCLCPP_ERROR( client_node_->get_logger( ), "Send goal call failed" );
return;
}
// Get the goal handle and save so that we can check on completion in the timer callback
waypoint_follower_goal_handle_ = future_goal_handle.get( );
if ( !waypoint_follower_goal_handle_ ) {
RCLCPP_ERROR( client_node_->get_logger( ), "Goal was rejected by server" );
return;
}
timer_.start( 200, this );
}
void
715 Nav2Panel::startNavThroughPoses( std::vector<geometry_msgs::msg::PoseStamped> poses )
{
auto is_action_server_ready =
nav_through_poses_action_client_->wait_for_action_server( std::chrono::seconds( 5 ) );
if ( !is_action_server_ready ) {
RCLCPP_ERROR(
client_node_->get_logger( ), "navigate_through_poses action server is not available."
" Is the initial pose set?" );
return;
}
nav_through_poses_goal_.poses = poses;
RCLCPP_INFO(
client_node_->get_logger( ),
"NavigateThroughPoses will be called using the BT Navigator's default behavior tree." );
RCLCPP_DEBUG(
client_node_->get_logger( ), "Sending a path of %zu waypoints:",
nav_through_poses_goal_.poses.size( ) );
for ( auto waypoint : nav_through_poses_goal_.poses ) {
RCLCPP_DEBUG(
client_node_->get_logger( ),
"\t( %lf, %lf )", waypoint.pose.position.x, waypoint.pose.position.y );
}
// Enable result awareness by providing an empty lambda function
auto send_goal_options =
rclcpp_action::Client<nav2_msgs::action::NavigateThroughPoses>::SendGoalOptions( );
send_goal_options.result_callback = [this]( auto ) {
nav_through_poses_goal_handle_.reset( );
};
auto future_goal_handle =
nav_through_poses_action_client_->async_send_goal( nav_through_poses_goal_, send_goal_options );
if ( rclcpp::spin_until_future_complete( client_node_, future_goal_handle, server_timeout_ ) !=
rclcpp::FutureReturnCode::SUCCESS )
{
RCLCPP_ERROR( client_node_->get_logger( ), "Send goal call failed" );
return;
}
// Get the goal handle and save so that we can check on completion in the timer callback
nav_through_poses_goal_handle_ = future_goal_handle.get( );
if ( !nav_through_poses_goal_handle_ ) {
RCLCPP_ERROR( client_node_->get_logger( ), "Goal was rejected by server" );
return;
}
timer_.start( 200, this );
}
void
767 Nav2Panel::startNavigation( geometry_msgs::msg::PoseStamped pose )
{
auto is_action_server_ready =
navigation_action_client_->wait_for_action_server( std::chrono::seconds( 5 ) );
if ( !is_action_server_ready ) {
RCLCPP_ERROR(
client_node_->get_logger( ),
"navigate_to_pose action server is not available."
" Is the initial pose set?" );
return;
}
// Send the goal pose
navigation_goal_.pose = pose;
RCLCPP_INFO(
client_node_->get_logger( ),
"NavigateToPose will be called using the BT Navigator's default behavior tree." );
// Enable result awareness by providing an empty lambda function
auto send_goal_options =
rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SendGoalOptions( );
send_goal_options.result_callback = [this]( auto ) {
navigation_goal_handle_.reset( );
};
auto future_goal_handle =
navigation_action_client_->async_send_goal( navigation_goal_, send_goal_options );
if ( rclcpp::spin_until_future_complete( client_node_, future_goal_handle, server_timeout_ ) !=
rclcpp::FutureReturnCode::SUCCESS )
{
RCLCPP_ERROR( client_node_->get_logger( ), "Send goal call failed" );
return;
}
// Get the goal handle and save so that we can check on completion in the timer callback
navigation_goal_handle_ = future_goal_handle.get( );
if ( !navigation_goal_handle_ ) {
RCLCPP_ERROR( client_node_->get_logger( ), "Goal was rejected by server" );
return;
}
timer_.start( 200, this );
}
void
813 Nav2Panel::save( rviz_common::Config config ) const
{
Panel::save( config );
}
void
819 Nav2Panel::load( const rviz_common::Config & config )
{
Panel::load( config );
}
void
825 Nav2Panel::resetUniqueId( )
{
unique_id = 0;
}
int
831 Nav2Panel::getUniqueId( )
{
int temp_id = unique_id;
unique_id += 1;
return temp_id;
}
void
839 Nav2Panel::updateWpNavigationMarkers( )
{
resetUniqueId( );
auto marker_array = std::make_unique<visualization_msgs::msg::MarkerArray>( );
for ( size_t i = 0; i < acummulated_poses_.size( ); i++ ) {
// Draw a green arrow at the waypoint pose
visualization_msgs::msg::Marker arrow_marker;
arrow_marker.header = acummulated_poses_[i].header;
arrow_marker.id = getUniqueId( );
arrow_marker.type = visualization_msgs::msg::Marker::ARROW;
arrow_marker.action = visualization_msgs::msg::Marker::ADD;
arrow_marker.pose = acummulated_poses_[i].pose;
arrow_marker.scale.x = 0.3;
arrow_marker.scale.y = 0.05;
arrow_marker.scale.z = 0.02;
arrow_marker.color.r = 0;
arrow_marker.color.g = 255;
arrow_marker.color.b = 0;
arrow_marker.color.a = 1.0f;
arrow_marker.lifetime = rclcpp::Duration( 0s );
arrow_marker.frame_locked = false;
marker_array->markers.push_back( arrow_marker );
// Draw a red circle at the waypoint pose
visualization_msgs::msg::Marker circle_marker;
circle_marker.header = acummulated_poses_[i].header;
circle_marker.id = getUniqueId( );
circle_marker.type = visualization_msgs::msg::Marker::SPHERE;
circle_marker.action = visualization_msgs::msg::Marker::ADD;
circle_marker.pose = acummulated_poses_[i].pose;
circle_marker.scale.x = 0.05;
circle_marker.scale.y = 0.05;
circle_marker.scale.z = 0.05;
circle_marker.color.r = 255;
circle_marker.color.g = 0;
circle_marker.color.b = 0;
circle_marker.color.a = 1.0f;
circle_marker.lifetime = rclcpp::Duration( 0s );
circle_marker.frame_locked = false;
marker_array->markers.push_back( circle_marker );
// Draw the waypoint number
visualization_msgs::msg::Marker marker_text;
marker_text.header = acummulated_poses_[i].header;
marker_text.id = getUniqueId( );
marker_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
marker_text.action = visualization_msgs::msg::Marker::ADD;
marker_text.pose = acummulated_poses_[i].pose;
marker_text.pose.position.z += 0.2; // draw it on top of the waypoint
marker_text.scale.x = 0.07;
marker_text.scale.y = 0.07;
marker_text.scale.z = 0.07;
marker_text.color.r = 0;
marker_text.color.g = 255;
marker_text.color.b = 0;
marker_text.color.a = 1.0f;
marker_text.lifetime = rclcpp::Duration( 0s );
marker_text.frame_locked = false;
marker_text.text = "wp_" + std::to_string( i + 1 );
marker_array->markers.push_back( marker_text );
}
if ( marker_array->markers.empty( ) ) {
visualization_msgs::msg::Marker clear_all_marker;
clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL;
marker_array->markers.push_back( clear_all_marker );
}
wp_navigation_markers_pub_->publish( std::move( marker_array ) );
}
inline QString
913 Nav2Panel::getGoalStatusLabel( int8_t status )
{
std::string status_str;
switch ( status ) {
case action_msgs::msg::GoalStatus::STATUS_EXECUTING:
status_str = "<font color=green>active</color>";
break;
case action_msgs::msg::GoalStatus::STATUS_SUCCEEDED:
status_str = "<font color=green>reached</color>";
break;
case action_msgs::msg::GoalStatus::STATUS_CANCELED:
status_str = "<font color=orange>canceled</color>";
break;
case action_msgs::msg::GoalStatus::STATUS_ABORTED:
status_str = "<font color=red>aborted</color>";
break;
case action_msgs::msg::GoalStatus::STATUS_UNKNOWN:
status_str = "unknown";
break;
default:
status_str = "inactive";
break;
}
return QString(
std::string(
"<table><tr><td width=100><b>Feedback:</b></td><td>" +
status_str + "</td></tr></table>" ).c_str( ) );
}
inline QString
948 Nav2Panel::getNavToPoseFeedbackLabel( nav2_msgs::action::NavigateToPose::Feedback msg )
{
return QString( std::string( "<table>" + toLabel( msg ) + "</table>" ).c_str( ) );
}
inline QString
954 Nav2Panel::getNavThroughPosesFeedbackLabel( nav2_msgs::action::NavigateThroughPoses::Feedback msg )
{
return QString(
std::string(
"<table><tr><td width=150>Poses remaining:</td><td>" +
std::to_string( msg.number_of_poses_remaining ) +
"</td></tr>" + toLabel( msg ) + "</table>" ).c_str( ) );
}
template<typename T>
964 inline std::string Nav2Panel::toLabel( T & msg )
{
return std::string(
"<tr><td width=150>ETA:</td><td>" +
toString( rclcpp::Duration( msg.estimated_time_remaining ).seconds( ), 0 ) + " s"
"</td></tr><tr><td width=150>Distance remaining:</td><td>" +
toString( msg.distance_remaining, 2 ) + " m"
"</td></tr><tr><td width=150>Time taken:</td><td>" +
toString( rclcpp::Duration( msg.navigation_time ).seconds( ), 0 ) + " s"
"</td></tr><tr><td width=150>Recoveries:</td><td>" +
std::to_string( msg.number_of_recoveries ) +
"</td></tr>" );
}
inline std::string
979 Nav2Panel::toString( double val, int precision )
{
std::ostringstream out;
out.precision( precision );
out << std::fixed << val;
return out.str( );
}
} // namespace nav2_rviz_plugins
#include <pluginlib/class_list_macros.hpp> // NOLINT
990 PLUGINLIB_EXPORT_CLASS( nav2_rviz_plugins::Nav2Panel, rviz_common::Panel )
1 /*
* Copyright ( c ) 2012, Willow Garage, Inc.
* Copyright ( c ) 2018, Bosch Software Innovations GmbH.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES ( INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE )
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
// Copyright ( c ) 2019 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp"
#include <vector>
#include <string>
#include <algorithm>
#include <OgreSceneManager.h>
#include <OgreTechnique.h>
#include "rviz_rendering/material_manager.hpp"
namespace nav2_rviz_plugins
{
60 FlatWeightedArrowsArray::FlatWeightedArrowsArray( Ogre::SceneManager * scene_manager )
: scene_manager_( scene_manager ), manual_object_( nullptr ) {}
63 FlatWeightedArrowsArray::~FlatWeightedArrowsArray( )
{
if ( manual_object_ ) {
scene_manager_->destroyManualObject( manual_object_ );
}
}
70 void FlatWeightedArrowsArray::createAndAttachManualObject( Ogre::SceneNode * scene_node )
{
manual_object_ = scene_manager_->createManualObject( );
manual_object_->setDynamic( true );
scene_node->attachObject( manual_object_ );
}
77 void FlatWeightedArrowsArray::updateManualObject(
78 Ogre::ColourValue color,
float alpha,
float min_length,
float max_length,
82 const std::vector<nav2_rviz_plugins::OgrePoseWithWeight> & poses )
{
clear( );
color.a = alpha;
setManualObjectMaterial( );
rviz_rendering::MaterialManager::enableAlphaBlending( material_, alpha );
manual_object_->begin(
material_->getName( ), Ogre::RenderOperation::OT_LINE_LIST, "rviz_rendering" );
setManualObjectVertices( color, min_length, max_length, poses );
manual_object_->end( );
}
96 void FlatWeightedArrowsArray::clear( )
{
if ( manual_object_ ) {
manual_object_->clear( );
}
}
103 void FlatWeightedArrowsArray::setManualObjectMaterial( )
{
static int material_count = 0;
std::string material_name = "FlatWeightedArrowsMaterial" + std::to_string( material_count++ );
material_ = rviz_rendering::MaterialManager::createMaterialWithNoLighting( material_name );
}
110 void FlatWeightedArrowsArray::setManualObjectVertices(
111 const Ogre::ColourValue & color,
float min_length,
float max_length,
114 const std::vector<nav2_rviz_plugins::OgrePoseWithWeight> & poses )
{
manual_object_->estimateVertexCount( poses.size( ) * 6 );
float scale = max_length - min_length;
float length;
for ( const auto & pose : poses ) {
length = std::min( std::max( pose.weight * scale + min_length, min_length ), max_length );
Ogre::Vector3 vertices[6];
vertices[0] = pose.position; // back of arrow
vertices[1] =
pose.position + pose.orientation * Ogre::Vector3( length, 0, 0 ); // tip of arrow
vertices[2] = vertices[1];
vertices[3] = pose.position + pose.orientation * Ogre::Vector3(
0.75f * length, 0.2f * length, 0 );
vertices[4] = vertices[1];
vertices[5] = pose.position + pose.orientation * Ogre::Vector3(
0.75f * length, -0.2f * length,
0 );
for ( const auto & vertex : vertices ) {
manual_object_->position( vertex );
manual_object_->colour( color );
}
}
}
} // namespace nav2_rviz_plugins
/*
* Copyright ( c ) 2012, Willow Garage, Inc.
* Copyright ( c ) 2018, Bosch Software Innovations GmbH.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES ( INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE )
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
// Copyright ( c ) 2019 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "nav2_rviz_plugins/particle_cloud_display/particle_cloud_display.hpp"
#include <memory>
#include <string>
#include <OgreManualObject.h>
#include <OgreMaterialManager.h>
#include <OgreTechnique.h>
#include "rviz_common/logging.hpp"
#include "rviz_common/msg_conversions.hpp"
#include "rviz_common/properties/enum_property.hpp"
#include "rviz_common/properties/color_property.hpp"
#include "rviz_common/properties/float_property.hpp"
#include "rviz_common/validate_floats.hpp"
#include "rviz_rendering/objects/arrow.hpp"
#include "rviz_rendering/objects/axes.hpp"
#include "nav2_rviz_plugins/particle_cloud_display/flat_weighted_arrows_array.hpp"
namespace nav2_rviz_plugins
{
namespace
{
struct ShapeType
{
enum
{
Arrow2d,
Arrow3d,
Axes,
};
};
} // namespace
83 ParticleCloudDisplay::ParticleCloudDisplay(
84 rviz_common::DisplayContext * display_context,
85 Ogre::SceneNode * scene_node )
: ParticleCloudDisplay( )
{
context_ = display_context;
scene_node_ = scene_node;
scene_manager_ = context_->getSceneManager( );
arrows2d_ = std::make_unique<FlatWeightedArrowsArray>( scene_manager_ );
arrows2d_->createAndAttachManualObject( scene_node );
arrow_node_ = scene_node_->createChildSceneNode( );
axes_node_ = scene_node_->createChildSceneNode( );
updateShapeChoice( );
}
99 ParticleCloudDisplay::ParticleCloudDisplay( )
100 : min_length_( 0.02f ), max_length_( 0.3f )
{
initializeProperties( );
shape_property_->addOption( "Arrow ( Flat )", ShapeType::Arrow2d );
shape_property_->addOption( "Arrow ( 3D )", ShapeType::Arrow3d );
shape_property_->addOption( "Axes", ShapeType::Axes );
arrow_alpha_property_->setMin( 0 );
arrow_alpha_property_->setMax( 1 );
arrow_min_length_property_->setMax( max_length_ );
arrow_max_length_property_->setMin( min_length_ );
}
113 void ParticleCloudDisplay::initializeProperties( )
{
shape_property_ = new rviz_common::properties::EnumProperty(
"Shape", "Arrow ( Flat )", "Shape to display the pose as.", this, SLOT( updateShapeChoice( ) ) );
arrow_color_property_ = new rviz_common::properties::ColorProperty(
"Color", QColor( 255, 25, 0 ), "Color to draw the arrows.", this, SLOT( updateArrowColor( ) ) );
arrow_alpha_property_ = new rviz_common::properties::FloatProperty(
"Alpha",
1.0f,
"Amount of transparency to apply to the displayed poses.",
this,
SLOT( updateArrowColor( ) ) );
arrow_min_length_property_ = new rviz_common::properties::FloatProperty(
"Min Arrow Length", min_length_, "Minimum length of the arrows.", this, SLOT( updateGeometry( ) ) );
arrow_max_length_property_ = new rviz_common::properties::FloatProperty(
"Max Arrow Length", max_length_, "Maximum length of the arrows.", this, SLOT( updateGeometry( ) ) );
// Scales are set based on initial values
length_scale_ = max_length_ - min_length_;
shaft_radius_scale_ = 0.0435;
head_length_scale_ = 0.3043;
head_radius_scale_ = 0.1304;
}
141 ParticleCloudDisplay::~ParticleCloudDisplay( )
{
// because of forward declaration of arrow and axes, destructor cannot be declared in .hpp as
// default
}
147 void ParticleCloudDisplay::onInitialize( )
{
MFDClass::onInitialize( );
arrows2d_ = std::make_unique<FlatWeightedArrowsArray>( scene_manager_ );
arrows2d_->createAndAttachManualObject( scene_node_ );
arrow_node_ = scene_node_->createChildSceneNode( );
axes_node_ = scene_node_->createChildSceneNode( );
updateShapeChoice( );
}
157 void ParticleCloudDisplay::processMessage( const nav2_msgs::msg::ParticleCloud::ConstSharedPtr msg )
{
if ( !validateFloats( *msg ) ) {
setStatus(
rviz_common::properties::StatusProperty::Error,
"Topic",
"Message contained invalid floating point values ( nans or infs )" );
return;
}
if ( !setTransform( msg->header ) ) {
return;
}
poses_.resize( msg->particles.size( ) );
for ( std::size_t i = 0; i < msg->particles.size( ); ++i ) {
poses_[i].position = rviz_common::pointMsgToOgre( msg->particles[i].pose.position );
poses_[i].orientation = rviz_common::quaternionMsgToOgre( msg->particles[i].pose.orientation );
poses_[i].weight = static_cast<float>( msg->particles[i].weight );
}
updateDisplay( );
context_->queueRender( );
}
184 bool ParticleCloudDisplay::validateFloats( const nav2_msgs::msg::ParticleCloud & msg )
{
for ( auto & particle : msg.particles ) {
if ( !rviz_common::validateFloats( particle.pose ) ||
!rviz_common::validateFloats( particle.weight ) )
{
return false;
}
}
return true;
}
196 bool ParticleCloudDisplay::setTransform( std_msgs::msg::Header const & header )
{
Ogre::Vector3 position;
Ogre::Quaternion orientation;
if ( !context_->getFrameManager( )->getTransform( header, position, orientation ) ) {
setMissingTransformToFixedFrame( header.frame_id );
return false;
}
setTransformOk( );
scene_node_->setPosition( position );
scene_node_->setOrientation( orientation );
return true;
}
211 void ParticleCloudDisplay::updateDisplay( )
{
int shape = shape_property_->getOptionInt( );
switch ( shape ) {
case ShapeType::Arrow2d:
updateArrows2d( );
arrows3d_.clear( );
axes_.clear( );
break;
case ShapeType::Arrow3d:
updateArrows3d( );
arrows2d_->clear( );
axes_.clear( );
break;
case ShapeType::Axes:
updateAxes( );
arrows2d_->clear( );
arrows3d_.clear( );
break;
}
}
233 void ParticleCloudDisplay::updateArrows2d( )
{
arrows2d_->updateManualObject(
arrow_color_property_->getOgreColor( ),
arrow_alpha_property_->getFloat( ),
min_length_,
max_length_,
poses_ );
}
243 void ParticleCloudDisplay::updateArrows3d( )
{
while ( arrows3d_.size( ) < poses_.size( ) ) {
arrows3d_.push_back( makeArrow3d( ) );
}
while ( arrows3d_.size( ) > poses_.size( ) ) {
arrows3d_.pop_back( );
}
Ogre::Quaternion adjust_orientation( Ogre::Degree( -90 ), Ogre::Vector3::UNIT_Y );
float shaft_length;
for ( std::size_t i = 0; i < poses_.size( ); ++i ) {
shaft_length = std::min(
std::max(
poses_[i].weight * length_scale_ + min_length_,
min_length_ ), max_length_ );
arrows3d_[i]->set(
shaft_length,
shaft_length * shaft_radius_scale_,
shaft_length * head_length_scale_,
shaft_length * head_radius_scale_
);
arrows3d_[i]->setPosition( poses_[i].position );
arrows3d_[i]->setOrientation( poses_[i].orientation * adjust_orientation );
}
}
270 void ParticleCloudDisplay::updateAxes( )
{
while ( axes_.size( ) < poses_.size( ) ) {
axes_.push_back( makeAxes( ) );
}
while ( axes_.size( ) > poses_.size( ) ) {
axes_.pop_back( );
}
float shaft_length;
for ( std::size_t i = 0; i < poses_.size( ); ++i ) {
shaft_length = std::min(
std::max(
poses_[i].weight * length_scale_ + min_length_,
min_length_ ), max_length_ );
axes_[i]->set( shaft_length, shaft_length * shaft_radius_scale_ );
axes_[i]->setPosition( poses_[i].position );
axes_[i]->setOrientation( poses_[i].orientation );
}
}
290 std::unique_ptr<rviz_rendering::Arrow> ParticleCloudDisplay::makeArrow3d( )
{
Ogre::ColourValue color = arrow_color_property_->getOgreColor( );
color.a = arrow_alpha_property_->getFloat( );
auto arrow = std::make_unique<rviz_rendering::Arrow>(
scene_manager_,
arrow_node_,
min_length_,
min_length_ * shaft_radius_scale_,
min_length_ * head_length_scale_,
min_length_ * head_radius_scale_
);
arrow->setColor( color );
return arrow;
}
308 std::unique_ptr<rviz_rendering::Axes> ParticleCloudDisplay::makeAxes( )
{
return std::make_unique<rviz_rendering::Axes>(
scene_manager_,
axes_node_,
min_length_,
min_length_ * shaft_radius_scale_
);
}
318 void ParticleCloudDisplay::reset( )
{
MFDClass::reset( );
arrows2d_->clear( );
arrows3d_.clear( );
axes_.clear( );
}
326 void ParticleCloudDisplay::updateShapeChoice( )
{
int shape = shape_property_->getOptionInt( );
bool use_axes = shape == ShapeType::Axes;
arrow_color_property_->setHidden( use_axes );
arrow_alpha_property_->setHidden( use_axes );
if ( initialized( ) ) {
updateDisplay( );
}
}
339 void ParticleCloudDisplay::updateArrowColor( )
{
int shape = shape_property_->getOptionInt( );
Ogre::ColourValue color = arrow_color_property_->getOgreColor( );
color.a = arrow_alpha_property_->getFloat( );
if ( shape == ShapeType::Arrow2d ) {
updateArrows2d( );
} else if ( shape == ShapeType::Arrow3d ) {
for ( const auto & arrow : arrows3d_ ) {
arrow->setColor( color );
}
}
context_->queueRender( );
}
355 void ParticleCloudDisplay::updateGeometry( )
{
min_length_ = arrow_min_length_property_->getFloat( );
max_length_ = arrow_max_length_property_->getFloat( );
length_scale_ = max_length_ - min_length_;
arrow_min_length_property_->setMax( max_length_ );
arrow_max_length_property_->setMin( min_length_ );
int shape = shape_property_->getOptionInt( );
switch ( shape ) {
case ShapeType::Arrow2d:
updateArrows2d( );
arrows3d_.clear( );
axes_.clear( );
break;
case ShapeType::Arrow3d:
updateArrow3dGeometry( );
arrows2d_->clear( );
axes_.clear( );
break;
case ShapeType::Axes:
updateAxesGeometry( );
arrows2d_->clear( );
arrows3d_.clear( );
break;
}
context_->queueRender( );
}
386 void ParticleCloudDisplay::updateArrow3dGeometry( )
{
float shaft_length;
for ( std::size_t i = 0; i < poses_.size( ) && i < arrows3d_.size( ); ++i ) {
shaft_length = std::min(
std::max(
poses_[i].weight * length_scale_ + min_length_,
min_length_ ), max_length_ );
arrows3d_[i]->set(
shaft_length,
shaft_length * shaft_radius_scale_,
shaft_length * head_length_scale_,
shaft_length * head_radius_scale_
);
}
}
403 void ParticleCloudDisplay::updateAxesGeometry( )
{
float shaft_length;
for ( std::size_t i = 0; i < poses_.size( ) && i < axes_.size( ); ++i ) {
shaft_length = std::min(
std::max(
poses_[i].weight * length_scale_ + min_length_,
min_length_ ), max_length_ );
axes_[i]->set( shaft_length, shaft_length * shaft_radius_scale_ );
}
}
415 void ParticleCloudDisplay::setShape( QString shape )
{
shape_property_->setValue( shape );
}
} // namespace nav2_rviz_plugins
#include <pluginlib/class_list_macros.hpp> // NOLINT
PLUGINLIB_EXPORT_CLASS( nav2_rviz_plugins::ParticleCloudDisplay, rviz_common::Display )
// Copyright ( c ) 2020, Samsung Research America
// Copyright ( c ) 2020, Applied Electric Vehicles Pty Ltd
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef NAV2_SMAC_PLANNER__A_STAR_HPP_
#define NAV2_SMAC_PLANNER__A_STAR_HPP_
#include <vector>
#include <iostream>
#include <unordered_map>
#include <memory>
#include <queue>
#include <utility>
#include "Eigen/Core"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_smac_planner/analytic_expansion.hpp"
#include "nav2_smac_planner/node_2d.hpp"
#include "nav2_smac_planner/node_hybrid.hpp"
#include "nav2_smac_planner/node_lattice.hpp"
#include "nav2_smac_planner/node_basic.hpp"
#include "nav2_smac_planner/types.hpp"
#include "nav2_smac_planner/constants.hpp"
namespace nav2_smac_planner
{
/**
* @class nav2_smac_planner::AStarAlgorithm
* @brief An A* implementation for planning in a costmap. Templated based on the Node type.
*/
template<typename NodeT>
45 class AStarAlgorithm
{
public:
typedef NodeT * NodePtr;
typedef std::unordered_map<unsigned int, NodeT> Graph;
typedef std::vector<NodePtr> NodeVector;
typedef std::pair<float, NodeBasic<NodeT>> NodeElement;
typedef typename NodeT::Coordinates Coordinates;
typedef typename NodeT::CoordinateVector CoordinateVector;
typedef typename NodeVector::iterator NeighborIterator;
typedef std::function<bool ( const unsigned int &, NodeT * & )> NodeGetter;
/**
* @struct nav2_smac_planner::NodeComparator
* @brief Node comparison for priority queue sorting
*/
struct NodeComparator
{
bool operator( )( const NodeElement & a, const NodeElement & b ) const
{
return a.first > b.first;
}
};
typedef std::priority_queue<NodeElement, std::vector<NodeElement>, NodeComparator> NodeQueue;
/**
* @brief A constructor for nav2_smac_planner::PlannerServer
* @param neighborhood The type of neighborhood to use for search ( 4 or 8 connected )
*/
75 explicit AStarAlgorithm( const MotionModel & motion_model, const SearchInfo & search_info );
/**
* @brief A destructor for nav2_smac_planner::AStarAlgorithm
*/
80 ~AStarAlgorithm( );
/**
* @brief Initialization of the planner with defaults
* @param allow_unknown Allow search in unknown space, good for navigation while mapping
* @param max_iterations Maximum number of iterations to use while expanding search
* @param max_on_approach_iterations Maximum number of iterations before returning a valid
* path once within thresholds to refine path
* comes at more compute time but smoother paths.
* @param max_planning_time Maximum time ( in seconds ) to wait for a plan, createPath returns
* false after this timeout
*/
92 void initialize(
const bool & allow_unknown,
int & max_iterations,
const int & max_on_approach_iterations,
const double & max_planning_time,
const float & lookup_table_size,
const unsigned int & dim_3_size );
/**
* @brief Creating path from given costmap, start, and goal
* @param path Reference to a vector of indicies of generated path
* @param num_iterations Reference to number of iterations to create plan
* @param tolerance Reference to tolerance in costmap nodes
* @return if plan was successful
*/
107 bool createPath( CoordinateVector & path, int & num_iterations, const float & tolerance );
/**
* @brief Sets the collision checker to use
* @param collision_checker Collision checker to use for checking state validity
*/
113 void setCollisionChecker( GridCollisionChecker * collision_checker );
/**
* @brief Set the goal for planning, as a node index
* @param mx The node X index of the goal
* @param my The node Y index of the goal
* @param dim_3 The node dim_3 index of the goal
*/
121 void setGoal(
const unsigned int & mx,
const unsigned int & my,
const unsigned int & dim_3 );
/**
* @brief Set the starting pose for planning, as a node index
* @param mx The node X index of the goal
* @param my The node Y index of the goal
* @param dim_3 The node dim_3 index of the goal
*/
132 void setStart(
const unsigned int & mx,
const unsigned int & my,
const unsigned int & dim_3 );
/**
* @brief Get maximum number of iterations to plan
* @return Reference to Maximum iterations parameter
*/
141 int & getMaxIterations( );
/**
* @brief Get pointer reference to starting node
* @return Node pointer reference to starting node
*/
147 NodePtr & getStart( );
/**
* @brief Get pointer reference to goal node
* @return Node pointer reference to goal node
*/
153 NodePtr & getGoal( );
/**
* @brief Get maximum number of on-approach iterations after within threshold
* @return Reference to Maximum on-appraoch iterations parameter
*/
159 int & getOnApproachMaxIterations( );
/**
* @brief Get tolerance, in node nodes
* @return Reference to tolerance parameter
*/
165 float & getToleranceHeuristic( );
/**
* @brief Get size of graph in X
* @return Size in X
*/
171 unsigned int & getSizeX( );
/**
* @brief Get size of graph in Y
* @return Size in Y
*/
177 unsigned int & getSizeY( );
/**
* @brief Get number of angle quantization bins ( SE2 ) or Z coordinate ( XYZ )
* @return Number of angle bins / Z dimension
*/
183 unsigned int & getSizeDim3( );
protected:
/**
* @brief Get pointer to next goal in open set
* @return Node pointer reference to next heuristically scored node
*/
190 inline NodePtr getNextNode( );
/**
* @brief Get pointer to next goal in open set
* @param cost The cost to sort into the open set of the node
* @param node Node pointer reference to add to open set
*/
197 inline void addNode( const float & cost, NodePtr & node );
/**
* @brief Adds node to graph
* @param cost The cost to sort into the open set of the node
* @param node Node pointer reference to add to open set
*/
204 inline NodePtr addToGraph( const unsigned int & index );
/**
* @brief Check if this node is the goal node
* @param node Node pointer to check if its the goal node
* @return if node is goal
*/
211 inline bool isGoal( NodePtr & node );
/**
* @brief Get cost of heuristic of node
* @param node Node index current
* @param node Node index of new
* @return Heuristic cost between the nodes
*/
219 inline float getHeuristicCost( const NodePtr & node );
/**
* @brief Check if inputs to planner are valid
* @return Are valid
*/
225 inline bool areInputsValid( );
/**
* @brief Clear hueristic queue of nodes to search
*/
230 inline void clearQueue( );
/**
* @brief Clear graph of nodes searched
*/
235 inline void clearGraph( );
int _timing_interval = 5000;
bool _traverse_unknown;
int _max_iterations;
int _max_on_approach_iterations;
double _max_planning_time;
float _tolerance;
unsigned int _x_size;
unsigned int _y_size;
unsigned int _dim3_size;
SearchInfo _search_info;
Coordinates _goal_coordinates;
NodePtr _start;
NodePtr _goal;
Graph _graph;
NodeQueue _queue;
MotionModel _motion_model;
NodeHeuristicPair _best_heuristic_node;
GridCollisionChecker * _collision_checker;
nav2_costmap_2d::Costmap2D * _costmap;
261 std::unique_ptr<AnalyticExpansion<NodeT>> _expander;
};
} // namespace nav2_smac_planner
#endif // NAV2_SMAC_PLANNER__A_STAR_HPP_
// Copyright ( c ) 2021, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_
#define NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_
#include <string>
#include <vector>
#include <list>
#include <memory>
#include "nav2_smac_planner/node_2d.hpp"
#include "nav2_smac_planner/node_hybrid.hpp"
#include "nav2_smac_planner/node_lattice.hpp"
#include "nav2_smac_planner/types.hpp"
#include "nav2_smac_planner/constants.hpp"
namespace nav2_smac_planner
{
template<typename NodeT>
33 class AnalyticExpansion
{
public:
typedef NodeT * NodePtr;
typedef typename NodeT::Coordinates Coordinates;
typedef std::function<bool ( const unsigned int &, NodeT * & )> NodeGetter;
/**
* @struct nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes
* @brief Analytic expansion nodes and associated metadata
*/
struct AnalyticExpansionNode
{
AnalyticExpansionNode(
NodePtr & node_in,
Coordinates & initial_coords_in,
Coordinates & proposed_coords_in )
: node( node_in ),
initial_coords( initial_coords_in ),
proposed_coords( proposed_coords_in )
{
}
NodePtr node;
Coordinates initial_coords;
Coordinates proposed_coords;
};
typedef std::vector<AnalyticExpansionNode> AnalyticExpansionNodes;
/**
* @brief Constructor for analytic expansion object
*/
66 AnalyticExpansion(
const MotionModel & motion_model,
const SearchInfo & search_info,
const bool & traverse_unknown,
const unsigned int & dim_3_size );
/**
* @brief Sets the collision checker and costmap to use in expansion validation
* @param collision_checker Collision checker to use
*/
76 void setCollisionChecker( GridCollisionChecker * collision_checker );
/**
* @brief Attempt an analytic path completion
* @param node The node to start the analytic path from
* @param goal The goal node to plan to
* @param getter Gets a node at a set of coordinates
* @param iterations Iterations to run over
* @param best_cost Best heuristic cost to propertionally expand more closer to the goal
* @return Node pointer reference to goal node if successful, else
* return nullptr
*/
88 NodePtr tryAnalyticExpansion(
const NodePtr & current_node,
const NodePtr & goal_node,
const NodeGetter & getter, int & iterations, int & best_cost );
/**
* @brief Perform an analytic path expansion to the goal
* @param node The node to start the analytic path from
* @param goal The goal node to plan to
* @param getter The function object that gets valid nodes from the graph
* @return A set of analytically expanded nodes to the goal from current node, if possible
*/
100 AnalyticExpansionNodes getAnalyticPath(
const NodePtr & node, const NodePtr & goal,
const NodeGetter & getter );
/**
* @brief Takes final analytic expansion and appends to current expanded node
* @param node The node to start the analytic path from
* @param goal The goal node to plan to
* @param expanded_nodes Expanded nodes to append to end of current search path
* @return Node pointer to goal node if successful, else return nullptr
*/
111 NodePtr setAnalyticPath(
const NodePtr & node, const NodePtr & goal,
const AnalyticExpansionNodes & expanded_nodes );
/**
* @brief Takes an expanded nodes to clean up, if necessary, of any state
* information that may be poluting it from a prior search iteration
* @param expanded_nodes Expanded node to clean up from search
*/
120 void cleanNode( const NodePtr & nodes );
protected:
MotionModel _motion_model;
SearchInfo _search_info;
bool _traverse_unknown;
unsigned int _dim_3_size;
GridCollisionChecker * _collision_checker;
std::list<std::unique_ptr<NodeT>> _detached_nodes;
};
} // namespace nav2_smac_planner
#endif // NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_
// Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <vector>
#include "nav2_costmap_2d/footprint_collision_checker.hpp"
#include "nav2_smac_planner/constants.hpp"
#ifndef NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_
#define NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_
namespace nav2_smac_planner
{
/**
* @class nav2_smac_planner::GridCollisionChecker
* @brief A costmap grid collision checker
*/
28 class GridCollisionChecker
29 : public nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>
{
public:
/**
* @brief A constructor for nav2_smac_planner::GridCollisionChecker
* for use when regular bin intervals are appropriate
* @param costmap The costmap to collision check against
* @param num_quantizations The number of quantizations to precompute footprint
* orientations for to speed up collision checking
*/
39 GridCollisionChecker(
40 nav2_costmap_2d::Costmap2D * costmap,
unsigned int num_quantizations );
/**
* @brief A constructor for nav2_smac_planner::GridCollisionChecker
* for use when irregular bin intervals are appropriate
* @param costmap The costmap to collision check against
* @param angles The vector of possible angle bins to precompute for
* orientations for to speed up collision checking, in radians
*/
// GridCollisionChecker(
// nav2_costmap_2d::Costmap2D * costmap,
// std::vector<float> & angles );
/**
* @brief Set the footprint to use with collision checker
* @param footprint The footprint to collision check against
* @param radius Whether or not the footprint is a circle and use radius collision checking
*/
59 void setFootprint(
60 const nav2_costmap_2d::Footprint & footprint,
61 const bool & radius,
const double & possible_inscribed_cost );
/**
* @brief Check if in collision with costmap and footprint at pose
* @param x X coordinate of pose to check against
* @param y Y coordinate of pose to check against
* @param theta Angle bin number of pose to check against ( NOT radians )
* @param traverse_unknown Whether or not to traverse in unknown space
* @return boolean if in collision or not.
*/
72 bool inCollision(
const float & x,
const float & y,
const float & theta,
76 const bool & traverse_unknown );
/**
* @brief Check if in collision with costmap and footprint at pose
* @param i Index to search collision status of
* @param traverse_unknown Whether or not to traverse in unknown space
* @return boolean if in collision or not.
*/
84 bool inCollision(
const unsigned int & i,
86 const bool & traverse_unknown );
/**
* @brief Get cost at footprint pose in costmap
* @return the cost at the pose in costmap
*/
92 float getCost( );
/**
* @brief Get the angles of the precomputed footprint orientations
* @return the ordered vector of angles corresponding to footprints
*/
98 std::vector<float> & getPrecomputedAngles( )
{
return angles_;
}
private:
/**
* @brief Check if value outside the range
* @param min Minimum value of the range
* @param max Maximum value of the range
* @param value the value to check if it is within the range
* @return boolean if in range or not
*/
111 bool outsideRange( const unsigned int & max, const float & value );
protected:
114 std::vector<nav2_costmap_2d::Footprint> oriented_footprints_;
115 nav2_costmap_2d::Footprint unoriented_footprint_;
double footprint_cost_;
117 bool footprint_is_radius_;
118 std::vector<float> angles_;
double possible_inscribed_cost_{-1};
};
} // namespace nav2_smac_planner
#endif // NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_
1 // Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef NAV2_SMAC_PLANNER__CONSTANTS_HPP_
#define NAV2_SMAC_PLANNER__CONSTANTS_HPP_
#include <string>
namespace nav2_smac_planner
{
22 enum class MotionModel
{
UNKNOWN = 0,
VON_NEUMANN = 1,
MOORE = 2,
DUBIN = 3,
REEDS_SHEPP = 4,
STATE_LATTICE = 5,
};
32 inline std::string toString( const MotionModel & n )
{
switch ( n ) {
case MotionModel::VON_NEUMANN:
return "Von Neumann";
case MotionModel::MOORE:
return "Moore";
case MotionModel::DUBIN:
return "Dubin";
case MotionModel::REEDS_SHEPP:
return "Reeds-Shepp";
case MotionModel::STATE_LATTICE:
return "State Lattice";
default:
return "Unknown";
}
}
50 inline MotionModel fromString( const std::string & n )
{
if ( n == "VON_NEUMANN" ) {
return MotionModel::VON_NEUMANN;
} else if ( n == "MOORE" ) {
return MotionModel::MOORE;
} else if ( n == "DUBIN" ) {
return MotionModel::DUBIN;
} else if ( n == "REEDS_SHEPP" ) {
return MotionModel::REEDS_SHEPP;
} else if ( n == "STATE_LATTICE" ) {
return MotionModel::STATE_LATTICE;
} else {
return MotionModel::UNKNOWN;
}
}
const float UNKNOWN = 255.0;
const float OCCUPIED = 254.0;
const float INSCRIBED = 253.0;
const float MAX_NON_OBSTACLE = 252.0;
const float FREE = 0;
} // namespace nav2_smac_planner
#endif // NAV2_SMAC_PLANNER__CONSTANTS_HPP_
1 // Copyright ( c ) 2020, Carlos Luis
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef NAV2_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_
#define NAV2_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_
#include <algorithm>
#include <string>
#include <memory>
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav2_smac_planner/constants.hpp"
namespace nav2_smac_planner
{
/**
* @class nav2_smac_planner::CostmapDownsampler
* @brief A costmap downsampler for more efficient path planning
*/
32 class CostmapDownsampler
{
public:
/**
* @brief A constructor for CostmapDownsampler
*/
38 CostmapDownsampler( );
/**
* @brief A destructor for CostmapDownsampler
*/
43 ~CostmapDownsampler( );
/**
* @brief Configure the downsampled costmap object and the ROS publisher
* @param node Lifecycle node pointer
* @param global_frame The ID of the global frame used by the costmap
* @param topic_name The name of the topic to publish the downsampled costmap
* @param costmap The costmap we want to downsample
* @param downsampling_factor Multiplier for the costmap resolution
* @param use_min_cost_neighbor If true, min function is used instead of max for downsampling
*/
54 void on_configure(
const nav2_util::LifecycleNode::WeakPtr & node,
const std::string & global_frame,
const std::string & topic_name,
nav2_costmap_2d::Costmap2D * const costmap,
const unsigned int & downsampling_factor,
const bool & use_min_cost_neighbor = false );
/**
* @brief Activate the publisher of the downsampled costmap
*/
65 void on_activate( );
/**
* @brief Deactivate the publisher of the downsampled costmap
*/
70 void on_deactivate( );
/**
* @brief Cleanup the publisher of the downsampled costmap
*/
75 void on_cleanup( );
/**
* @brief Downsample the given costmap by the downsampling factor, and publish the downsampled costmap
* @param downsampling_factor Multiplier for the costmap resolution
* @return A ptr to the downsampled costmap
*/
82 nav2_costmap_2d::Costmap2D * downsample( const unsigned int & downsampling_factor );
/**
* @brief Resize the downsampled costmap. Used in case the costmap changes and we need to update the downsampled version
*/
87 void resizeCostmap( );
protected:
/**
* @brief Update the sizes X-Y of the costmap and its downsampled version
*/
93 void updateCostmapSize( );
/**
* @brief Explore all subcells of the original costmap and assign the max cost to the new ( downsampled ) cell
* @param new_mx The X-coordinate of the cell in the new costmap
* @param new_my The Y-coordinate of the cell in the new costmap
*/
100 void setCostOfCell(
const unsigned int & new_mx,
const unsigned int & new_my );
unsigned int _size_x;
unsigned int _size_y;
unsigned int _downsampled_size_x;
unsigned int _downsampled_size_y;
unsigned int _downsampling_factor;
109 bool _use_min_cost_neighbor;
float _downsampled_resolution;
111 nav2_costmap_2d::Costmap2D * _costmap;
112 std::unique_ptr<nav2_costmap_2d::Costmap2D> _downsampled_costmap;
113 std::unique_ptr<nav2_costmap_2d::Costmap2DPublisher> _downsampled_costmap_pub;
};
} // namespace nav2_smac_planner
#endif // NAV2_SMAC_PLANNER__COSTMAP_DOWNSAMPLER_HPP_
// Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef NAV2_SMAC_PLANNER__NODE_2D_HPP_
#define NAV2_SMAC_PLANNER__NODE_2D_HPP_
#include <math.h>
#include <vector>
#include <iostream>
#include <memory>
#include <queue>
#include <limits>
#include <utility>
#include <functional>
#include "nav2_smac_planner/types.hpp"
#include "nav2_smac_planner/constants.hpp"
#include "nav2_smac_planner/collision_checker.hpp"
#include "nav2_smac_planner/node_hybrid.hpp"
namespace nav2_smac_planner
{
/**
* @class nav2_smac_planner::Node2D
* @brief Node2D implementation for graph
*/
39 class Node2D
{
public:
typedef Node2D * NodePtr;
typedef std::unique_ptr<std::vector<Node2D>> Graph;
typedef std::vector<NodePtr> NodeVector;
/**
* @class nav2_smac_planner::Node2D::Coordinates
* @brief Node2D implementation of coordinate structure
*/
struct Coordinates
{
Coordinates( ) {}
Coordinates( const float & x_in, const float & y_in )
: x( x_in ), y( y_in )
{}
float x, y;
};
typedef std::vector<Coordinates> CoordinateVector;
/**
* @brief A constructor for nav2_smac_planner::Node2D
* @param index The index of this node for self-reference
*/
explicit Node2D( const unsigned int index );
/**
* @brief A destructor for nav2_smac_planner::Node2D
*/
~Node2D( );
/**
* @brief operator== for comparisons
* @param Node2D right hand side node reference
* @return If cell indicies are equal
*/
bool operator==( const Node2D & rhs )
{
return this->_index == rhs._index;
}
/**
* @brief Reset method for new search
*/
void reset( );
/**
* @brief Gets the accumulated cost at this node
* @return accumulated cost
*/
inline float & getAccumulatedCost( )
{
return _accumulated_cost;
}
/**
* @brief Sets the accumulated cost at this node
* @param reference to accumulated cost
*/
inline void setAccumulatedCost( const float & cost_in )
{
_accumulated_cost = cost_in;
}
/**
* @brief Gets the costmap cost at this node
* @return costmap cost
*/
inline float & getCost( )
{
return _cell_cost;
}
/**
* @brief Gets the costmap cost at this node
* @return costmap cost
*/
inline void setCost( const float & cost )
{
_cell_cost = cost;
}
/**
* @brief Gets if cell has been visited in search
* @param If cell was visited
*/
inline bool & wasVisited( )
{
return _was_visited;
}
/**
* @brief Sets if cell has been visited in search
*/
inline void visited( )
{
_was_visited = true;
_is_queued = false;
}
/**
* @brief Gets if cell is currently queued in search
* @param If cell was queued
*/
inline bool & isQueued( )
{
return _is_queued;
}
/**
* @brief Sets if cell is currently queued in search
*/
inline void queued( )
{
_is_queued = true;
}
/**
* @brief Gets cell index
* @return Reference to cell index
*/
inline unsigned int & getIndex( )
{
return _index;
}
/**
* @brief Check if this node is valid
* @param traverse_unknown If we can explore unknown nodes on the graph
* @param collision_checker Pointer to collision checker object
* @return whether this node is valid and collision free
*/
bool isNodeValid( const bool & traverse_unknown, GridCollisionChecker * collision_checker );
/**
* @brief get traversal cost from this node to child node
* @param child Node pointer to this node's child
* @return traversal cost
*/
float getTraversalCost( const NodePtr & child );
/**
* @brief Get index
* @param x x coordinate of point to get index of
* @param y y coordinate of point to get index of
* @param width width of costmap
* @return index
*/
static inline unsigned int getIndex(
const unsigned int & x, const unsigned int & y, const unsigned int & width )
{
return x + y * width;
}
/**
* @brief Get index
* @param Index Index of point
* @param width width of costmap
* @param angles angle bins to use ( must be 1 or throws exception )
* @return coordinates of point
*/
static inline Coordinates getCoords(
const unsigned int & index, const unsigned int & width, const unsigned int & angles )
{
if ( angles != 1 ) {
throw std::runtime_error( "Node type Node2D does not have a valid angle quantization." );
}
return Coordinates( index % width, index / width );
}
/**
* @brief Get index
* @param Index Index of point
* @return coordinates of point
*/
static inline Coordinates getCoords( const unsigned int & index )
{
const unsigned int & size_x = _neighbors_grid_offsets[3];
return Coordinates( index % size_x, index / size_x );
}
/**
* @brief Get cost of heuristic of node
* @param node Node index current
* @param node Node index of new
* @param costmap Costmap ptr to use
* @return Heuristic cost between the nodes
*/
static float getHeuristicCost(
const Coordinates & node_coords,
const Coordinates & goal_coordinates,
const nav2_costmap_2d::Costmap2D * costmap );
/**
* @brief Initialize the neighborhood to be used in A*
* We support 4-connect ( VON_NEUMANN ) and 8-connect ( MOORE )
* @param neighborhood The desired neighborhood type
* @param x_size_uint The total x size to find neighbors
* @param y_size The total y size to find neighbors
* @param num_angle_quantization Number of quantizations, must be 0
* @param search_info Search parameters, unused by 2D node
*/
static void initMotionModel(
const MotionModel & motion_model,
unsigned int & size_x,
unsigned int & size_y,
unsigned int & num_angle_quantization,
SearchInfo & search_info );
/**
* @brief Retrieve all valid neighbors of a node.
* @param validity_checker Functor for state validity checking
* @param collision_checker Collision checker to use
* @param traverse_unknown If unknown costs are valid to traverse
* @param neighbors Vector of neighbors to be filled
*/
void getNeighbors(
std::function<bool( const unsigned int &, nav2_smac_planner::Node2D * & )> & validity_checker,
GridCollisionChecker * collision_checker,
const bool & traverse_unknown,
NodeVector & neighbors );
/**
* @brief Set the starting pose for planning, as a node index
* @param path Reference to a vector of indicies of generated path
* @return whether the path was able to be backtraced
*/
bool backtracePath( CoordinateVector & path );
Node2D * parent;
static float cost_travel_multiplier;
static std::vector<int> _neighbors_grid_offsets;
private:
float _cell_cost;
float _accumulated_cost;
unsigned int _index;
bool _was_visited;
bool _is_queued;
};
} // namespace nav2_smac_planner
#endif // NAV2_SMAC_PLANNER__NODE_2D_HPP_
1 // Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef NAV2_SMAC_PLANNER__NODE_BASIC_HPP_
#define NAV2_SMAC_PLANNER__NODE_BASIC_HPP_
#include <math.h>
#include <vector>
#include <cmath>
#include <iostream>
#include <functional>
#include <queue>
#include <memory>
#include <utility>
#include <limits>
#include "ompl/base/StateSpace.h"
#include "nav2_smac_planner/constants.hpp"
#include "nav2_smac_planner/node_hybrid.hpp"
#include "nav2_smac_planner/node_lattice.hpp"
#include "nav2_smac_planner/node_2d.hpp"
#include "nav2_smac_planner/types.hpp"
#include "nav2_smac_planner/collision_checker.hpp"
namespace nav2_smac_planner
{
/**
* @class nav2_smac_planner::NodeBasic
* @brief NodeBasic implementation for priority queue insertion
*/
template<typename NodeT>
45 class NodeBasic
{
public:
/**
* @brief A constructor for nav2_smac_planner::NodeBasic
* @param cost_in The costmap cost at this node
* @param index The index of this node for self-reference
*/
53 explicit NodeBasic( const unsigned int index )
: index( index ),
graph_node_ptr( nullptr )
{
}
/**
* @brief Take a NodeBasic and populate it with any necessary state
* cached in the queue for NodeT.
* @param node NodeT ptr to populate metadata into NodeBasic
*/
64 void populateSearchNode( NodeT * & node );
/**
* @brief Take a NodeBasic and populate it with any necessary state
* cached in the queue for NodeTs.
* @param node Search node ( basic ) object to initialize internal node
* with state
*/
72 void processSearchNode( );
typename NodeT::Coordinates pose; // Used by NodeHybrid and NodeLattice
NodeT * graph_node_ptr;
MotionPrimitive * prim_ptr; // Used by NodeLattice
unsigned int index, motion_index;
bool backward;
};
} // namespace nav2_smac_planner
#endif // NAV2_SMAC_PLANNER__NODE_BASIC_HPP_
// Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_
#define NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_
#include <math.h>
#include <vector>
#include <cmath>
#include <iostream>
#include <functional>
#include <queue>
#include <memory>
#include <utility>
#include <limits>
#include "ompl/base/StateSpace.h"
#include "nav2_smac_planner/constants.hpp"
#include "nav2_smac_planner/types.hpp"
#include "nav2_smac_planner/collision_checker.hpp"
#include "nav2_smac_planner/costmap_downsampler.hpp"
namespace nav2_smac_planner
{
typedef std::vector<float> LookupTable;
typedef std::pair<double, double> TrigValues;
typedef std::pair<float, unsigned int> ObstacleHeuristicElement;
struct ObstacleHeuristicComparator
{
bool operator( )( const ObstacleHeuristicElement & a, const ObstacleHeuristicElement & b ) const
{
return a.first > b.first;
}
};
typedef std::vector<ObstacleHeuristicElement> ObstacleHeuristicQueue;
// Must forward declare
53 class NodeHybrid;
/**
* @struct nav2_smac_planner::HybridMotionTable
* @brief A table of motion primitives and related functions
*/
struct HybridMotionTable
{
/**
* @brief A constructor for nav2_smac_planner::HybridMotionTable
*/
HybridMotionTable( ) {}
/**
* @brief Initializing using Dubin model
* @param size_x_in Size of costmap in X
* @param size_y_in Size of costmap in Y
* @param angle_quantization_in Size of costmap in bin sizes
* @param search_info Parameters for searching
*/
void initDubin(
unsigned int & size_x_in,
unsigned int & size_y_in,
unsigned int & angle_quantization_in,
SearchInfo & search_info );
/**
* @brief Initializing using Reeds-Shepp model
* @param size_x_in Size of costmap in X
* @param size_y_in Size of costmap in Y
* @param angle_quantization_in Size of costmap in bin sizes
* @param search_info Parameters for searching
*/
void initReedsShepp(
unsigned int & size_x_in,
unsigned int & size_y_in,
unsigned int & angle_quantization_in,
SearchInfo & search_info );
/**
* @brief Get projections of motion models
* @param node Ptr to NodeHybrid
* @return A set of motion poses
*/
MotionPoses getProjections( const NodeHybrid * node );
/**
* @brief Get the angular bin to use from a raw orientation
* @param theta Angle in radians
* @return bin index of closest angle to request
*/
unsigned int getClosestAngularBin( const double & theta );
/**
* @brief Get the raw orientation from an angular bin
* @param bin_idx Index of the bin
* @return Raw orientation in radians
*/
float getAngleFromBin( const unsigned int & bin_idx );
MotionModel motion_model = MotionModel::UNKNOWN;
MotionPoses projections;
unsigned int size_x;
unsigned int num_angle_quantization;
float num_angle_quantization_float;
float min_turning_radius;
float bin_size;
float change_penalty;
float non_straight_penalty;
float cost_penalty;
float reverse_penalty;
float travel_distance_reward;
ompl::base::StateSpacePtr state_space;
std::vector<std::vector<double>> delta_xs;
std::vector<std::vector<double>> delta_ys;
std::vector<TrigValues> trig_values;
};
/**
* @class nav2_smac_planner::NodeHybrid
* @brief NodeHybrid implementation for graph, Hybrid-A*
*/
135 class NodeHybrid
{
public:
typedef NodeHybrid * NodePtr;
typedef std::unique_ptr<std::vector<NodeHybrid>> Graph;
typedef std::vector<NodePtr> NodeVector;
/**
* @class nav2_smac_planner::NodeHybrid::Coordinates
* @brief NodeHybrid implementation of coordinate structure
*/
struct Coordinates
{
/**
* @brief A constructor for nav2_smac_planner::NodeHybrid::Coordinates
*/
Coordinates( ) {}
/**
* @brief A constructor for nav2_smac_planner::NodeHybrid::Coordinates
* @param x_in X coordinate
* @param y_in Y coordinate
* @param theta_in Theta coordinate
*/
Coordinates( const float & x_in, const float & y_in, const float & theta_in )
: x( x_in ), y( y_in ), theta( theta_in )
{}
163 inline bool operator==( const Coordinates & rhs )
{
return this->x == rhs.x && this->y == rhs.y && this->theta == rhs.theta;
}
168 inline bool operator!=( const Coordinates & rhs )
{
return !( *this == rhs );
}
float x, y, theta;
};
typedef std::vector<Coordinates> CoordinateVector;
/**
* @brief A constructor for nav2_smac_planner::NodeHybrid
* @param index The index of this node for self-reference
*/
explicit NodeHybrid( const unsigned int index );
/**
* @brief A destructor for nav2_smac_planner::NodeHybrid
*/
~NodeHybrid( );
/**
* @brief operator== for comparisons
* @param NodeHybrid right hand side node reference
* @return If cell indicies are equal
*/
bool operator==( const NodeHybrid & rhs )
{
return this->_index == rhs._index;
}
/**
* @brief setting continuous coordinate search poses ( in partial-cells )
* @param Pose pose
*/
inline void setPose( const Coordinates & pose_in )
{
pose = pose_in;
}
/**
* @brief Reset method for new search
*/
void reset( );
/**
* @brief Gets the accumulated cost at this node
* @return accumulated cost
*/
inline float & getAccumulatedCost( )
{
return _accumulated_cost;
}
/**
* @brief Sets the accumulated cost at this node
* @param reference to accumulated cost
*/
inline void setAccumulatedCost( const float & cost_in )
{
_accumulated_cost = cost_in;
}
/**
* @brief Sets the motion primitive index used to achieve node in search
* @param reference to motion primitive idx
*/
inline void setMotionPrimitiveIndex( const unsigned int & idx )
{
_motion_primitive_index = idx;
}
/**
* @brief Gets the motion primitive index used to achieve node in search
* @return reference to motion primitive idx
*/
inline unsigned int & getMotionPrimitiveIndex( )
{
return _motion_primitive_index;
}
/**
* @brief Gets the costmap cost at this node
* @return costmap cost
*/
inline float & getCost( )
{
return _cell_cost;
}
/**
* @brief Gets if cell has been visited in search
* @param If cell was visited
*/
inline bool & wasVisited( )
{
return _was_visited;
}
/**
* @brief Sets if cell has been visited in search
*/
inline void visited( )
{
_was_visited = true;
}
/**
* @brief Gets cell index
* @return Reference to cell index
*/
inline unsigned int & getIndex( )
{
return _index;
}
/**
* @brief Check if this node is valid
* @param traverse_unknown If we can explore unknown nodes on the graph
* @return whether this node is valid and collision free
*/
bool isNodeValid( const bool & traverse_unknown, GridCollisionChecker * collision_checker );
/**
* @brief Get traversal cost of parent node to child node
* @param child Node pointer to child
* @return traversal cost
*/
float getTraversalCost( const NodePtr & child );
/**
* @brief Get index at coordinates
* @param x X coordinate of point
* @param y Y coordinate of point
* @param angle Theta coordinate of point
* @param width Width of costmap
* @param angle_quantization Number of theta bins
* @return Index
*/
static inline unsigned int getIndex(
const unsigned int & x, const unsigned int & y, const unsigned int & angle,
const unsigned int & width, const unsigned int & angle_quantization )
{
return angle + x * angle_quantization + y * width * angle_quantization;
}
/**
* @brief Get index at coordinates
* @param x X coordinate of point
* @param y Y coordinate of point
* @param angle Theta coordinate of point
* @return Index
*/
static inline unsigned int getIndex(
const unsigned int & x, const unsigned int & y, const unsigned int & angle )
{
return getIndex(
x, y, angle, motion_table.size_x,
motion_table.num_angle_quantization );
}
/**
* @brief Get coordinates at index
* @param index Index of point
* @param width Width of costmap
* @param angle_quantization Theta size of costmap
* @return Coordinates
*/
static inline Coordinates getCoords(
const unsigned int & index,
const unsigned int & width, const unsigned int & angle_quantization )
{
return Coordinates(
( index / angle_quantization ) % width, // x
index / ( angle_quantization * width ), // y
index % angle_quantization ); // theta
}
/**
* @brief Get cost of heuristic of node
* @param node Node index current
* @param node Node index of new
* @param costmap Costmap ptr to use
* @return Heuristic cost between the nodes
*/
static float getHeuristicCost(
const Coordinates & node_coords,
const Coordinates & goal_coordinates,
const nav2_costmap_2d::Costmap2D * costmap );
/**
* @brief Initialize motion models
* @param motion_model Motion model enum to use
* @param size_x Size of X of graph
* @param size_y Size of y of graph
* @param angle_quantization Size of theta bins of graph
* @param search_info Search info to use
*/
static void initMotionModel(
const MotionModel & motion_model,
unsigned int & size_x,
unsigned int & size_y,
unsigned int & angle_quantization,
SearchInfo & search_info );
/**
* @brief Compute the SE2 distance heuristic
* @param lookup_table_dim Size, in costmap pixels, of the
* each lookup table dimension to populate
* @param motion_model Motion model to use for state space
* @param dim_3_size Number of quantization bins for caching
* @param search_info Info containing minimum radius to use
*/
static void precomputeDistanceHeuristic(
const float & lookup_table_dim,
const MotionModel & motion_model,
const unsigned int & dim_3_size,
const SearchInfo & search_info );
/**
* @brief Compute the Obstacle heuristic
* @param node_coords Coordinates to get heuristic at
* @param goal_coords Coordinates to compute heuristic to
* @return heuristic Heuristic value
*/
static float getObstacleHeuristic(
const Coordinates & node_coords,
const Coordinates & goal_coords,
const double & cost_penalty );
/**
* @brief Compute the Distance heuristic
* @param node_coords Coordinates to get heuristic at
* @param goal_coords Coordinates to compute heuristic to
* @param obstacle_heuristic Value of the obstacle heuristic to compute
* additional motion heuristics if required
* @return heuristic Heuristic value
*/
static float getDistanceHeuristic(
const Coordinates & node_coords,
const Coordinates & goal_coords,
const float & obstacle_heuristic );
/**
* @brief reset the obstacle heuristic state
* @param costmap Costmap to use
* @param goal_coords Coordinates to start heuristic expansion at
*/
static void resetObstacleHeuristic(
nav2_costmap_2d::Costmap2D * costmap,
const unsigned int & start_x, const unsigned int & start_y,
const unsigned int & goal_x, const unsigned int & goal_y );
/**
* @brief Retrieve all valid neighbors of a node.
* @param validity_checker Functor for state validity checking
* @param collision_checker Collision checker to use
* @param traverse_unknown If unknown costs are valid to traverse
* @param neighbors Vector of neighbors to be filled
*/
void getNeighbors(
std::function<bool( const unsigned int &, nav2_smac_planner::NodeHybrid * & )> & validity_checker,
GridCollisionChecker * collision_checker,
const bool & traverse_unknown,
NodeVector & neighbors );
/**
* @brief Set the starting pose for planning, as a node index
* @param path Reference to a vector of indicies of generated path
* @return whether the path was able to be backtraced
*/
bool backtracePath( CoordinateVector & path );
NodeHybrid * parent;
Coordinates pose;
// Constants required across all nodes but don't want to allocate more than once
static double travel_distance_cost;
static HybridMotionTable motion_table;
// Wavefront lookup and queue for continuing to expand as needed
static LookupTable obstacle_heuristic_lookup_table;
static ObstacleHeuristicQueue obstacle_heuristic_queue;
static nav2_costmap_2d::Costmap2D * sampled_costmap;
static CostmapDownsampler downsampler;
// Dubin / Reeds-Shepp lookup and size for dereferencing
static LookupTable dist_heuristic_lookup_table;
static float size_lookup;
private:
float _cell_cost;
float _accumulated_cost;
unsigned int _index;
bool _was_visited;
unsigned int _motion_primitive_index;
};
} // namespace nav2_smac_planner
#endif // NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_
// Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef NAV2_SMAC_PLANNER__NODE_LATTICE_HPP_
#define NAV2_SMAC_PLANNER__NODE_LATTICE_HPP_
#include <math.h>
#include <vector>
#include <cmath>
#include <iostream>
#include <functional>
#include <queue>
#include <memory>
#include <utility>
#include <limits>
#include <string>
#include "nlohmann/json.hpp"
#include "ompl/base/StateSpace.h"
#include "angles/angles.h"
#include "nav2_smac_planner/constants.hpp"
#include "nav2_smac_planner/types.hpp"
#include "nav2_smac_planner/collision_checker.hpp"
#include "nav2_smac_planner/node_hybrid.hpp"
#include "nav2_smac_planner/utils.hpp"
namespace nav2_smac_planner
{
// forward declare
44 class NodeLattice;
45 class NodeHybrid;
/**
* @struct nav2_smac_planner::LatticeMotionTable
* @brief A table of motion primitives and related functions
*/
struct LatticeMotionTable
{
/**
* @brief A constructor for nav2_smac_planner::LatticeMotionTable
*/
LatticeMotionTable( ) {}
/**
* @brief Initializing state lattice planner's motion model
* @param size_x_in Size of costmap in X
* @param size_y_in Size of costmap in Y
* @param angle_quantization_in Size of costmap in bin sizes
* @param search_info Parameters for searching
*/
void initMotionModel(
unsigned int & size_x_in,
SearchInfo & search_info );
/**
* @brief Get projections of motion models
* @param node Ptr to NodeLattice
* @return A set of motion poses
*/
MotionPrimitivePtrs getMotionPrimitives( const NodeLattice * node );
/**
* @brief Get file metadata needed
* @param lattice_filepath Filepath to the lattice file
* @return A set of metadata containing the number of angular bins
* and the global coordinates minimum turning radius of the primitives
* for use in analytic expansion and heuristic calculation.
*/
static LatticeMetadata getLatticeMetadata( const std::string & lattice_filepath );
/**
* @brief Get the angular bin to use from a raw orientation
* @param theta Angle in radians
* @return bin index of closest angle to request
*/
unsigned int getClosestAngularBin( const double & theta );
/**
* @brief Get the raw orientation from an angular bin
* @param bin_idx Index of the bin
* @return Raw orientation in radians
*/
float & getAngleFromBin( const unsigned int & bin_idx );
unsigned int size_x;
unsigned int num_angle_quantization;
float change_penalty;
float non_straight_penalty;
float cost_penalty;
float reverse_penalty;
float travel_distance_reward;
float rotation_penalty;
bool allow_reverse_expansion;
std::vector<std::vector<MotionPrimitive>> motion_primitives;
ompl::base::StateSpacePtr state_space;
std::vector<TrigValues> trig_values;
std::string current_lattice_filepath;
LatticeMetadata lattice_metadata;
};
/**
* @class nav2_smac_planner::NodeLattice
* @brief NodeLattice implementation for graph, Hybrid-A*
*/
119 class NodeLattice
{
public:
typedef NodeLattice * NodePtr;
typedef std::unique_ptr<std::vector<NodeLattice>> Graph;
typedef std::vector<NodePtr> NodeVector;
typedef NodeHybrid::Coordinates Coordinates;
typedef NodeHybrid::CoordinateVector CoordinateVector;
/**
* @brief A constructor for nav2_smac_planner::NodeLattice
* @param index The index of this node for self-reference
*/
explicit NodeLattice( const unsigned int index );
/**
* @brief A destructor for nav2_smac_planner::NodeLattice
*/
~NodeLattice( );
/**
* @brief operator== for comparisons
* @param NodeLattice right hand side node reference
* @return If cell indicies are equal
*/
bool operator==( const NodeLattice & rhs )
{
return this->_index == rhs._index;
}
/**
* @brief setting continuous coordinate search poses ( in partial-cells )
* @param Pose pose
*/
inline void setPose( const Coordinates & pose_in )
{
pose = pose_in;
}
/**
* @brief Reset method for new search
*/
161 void reset( );
/**
* @brief Sets the motion primitive used to achieve node in search
* @param pointer to motion primitive
*/
167 inline void setMotionPrimitive( MotionPrimitive * prim )
{
_motion_primitive = prim;
}
/**
* @brief Gets the motion primitive used to achieve node in search
* @return pointer to motion primitive
*/
176 inline MotionPrimitive * & getMotionPrimitive( )
{
return _motion_primitive;
}
/**
* @brief Gets the accumulated cost at this node
* @return accumulated cost
*/
185 inline float & getAccumulatedCost( )
{
return _accumulated_cost;
}
/**
* @brief Sets the accumulated cost at this node
* @param reference to accumulated cost
*/
194 inline void setAccumulatedCost( const float & cost_in )
{
_accumulated_cost = cost_in;
}
/**
* @brief Gets the costmap cost at this node
* @return costmap cost
*/
203 inline float & getCost( )
{
return _cell_cost;
}
/**
* @brief Gets if cell has been visited in search
* @param If cell was visited
*/
212 inline bool & wasVisited( )
{
return _was_visited;
}
/**
* @brief Sets if cell has been visited in search
*/
220 inline void visited( )
{
_was_visited = true;
}
/**
* @brief Gets cell index
* @return Reference to cell index
*/
229 inline unsigned int & getIndex( )
{
return _index;
}
/**
* @brief Sets that this primitive is moving in reverse
*/
237 inline void backwards( bool back = true )
{
_backwards = back;
}
/**
* @brief Gets if this primitive is moving in reverse
* @return backwards If moving in reverse
*/
246 inline bool isBackward( )
{
return _backwards;
}
/**
* @brief Check if this node is valid
* @param traverse_unknown If we can explore unknown nodes on the graph
* @param collision_checker Collision checker object to aid in validity checking
* @param primitive Optional argument if needing to check over a primitive
* not only a terminal pose
* @param is_backwards Optional argument if needed to check if prim expansion is
* in reverse
* @return whether this node is valid and collision free
*/
261 bool isNodeValid(
const bool & traverse_unknown,
GridCollisionChecker * collision_checker,
MotionPrimitive * primitive = nullptr,
bool is_backwards = false );
/**
* @brief Get traversal cost of parent node to child node
* @param child Node pointer to child
* @return traversal cost
*/
272 float getTraversalCost( const NodePtr & child );
/**
* @brief Get index at coordinates
* @param x X coordinate of point
* @param y Y coordinate of point
* @param angle Theta coordinate of point
* @return Index
*/
281 static inline unsigned int getIndex(
const unsigned int & x, const unsigned int & y, const unsigned int & angle )
{
// Hybrid-A* and State Lattice share a coordinate system
return NodeHybrid::getIndex(
x, y, angle, motion_table.size_x,
motion_table.num_angle_quantization );
}
/**
* @brief Get coordinates at index
* @param index Index of point
* @param width Width of costmap
* @param angle_quantization Theta size of costmap
* @return Coordinates
*/
297 static inline Coordinates getCoords(
const unsigned int & index,
const unsigned int & width, const unsigned int & angle_quantization )
{
// Hybrid-A* and State Lattice share a coordinate system
return NodeHybrid::Coordinates(
( index / angle_quantization ) % width, // x
index / ( angle_quantization * width ), // y
index % angle_quantization ); // theta
}
/**
* @brief Get cost of heuristic of node
* @param node Node index current
* @param node Node index of new
* @param costmap Costmap ptr to use
* @return Heuristic cost between the nodes
*/
315 static float getHeuristicCost(
const Coordinates & node_coords,
const Coordinates & goal_coordinates,
const nav2_costmap_2d::Costmap2D * costmap );
/**
* @brief Initialize motion models
* @param motion_model Motion model enum to use
* @param size_x Size of X of graph
* @param size_y Size of y of graph
* @param angle_quantization Size of theta bins of graph
* @param search_info Search info to use
*/
328 static void initMotionModel(
const MotionModel & motion_model,
unsigned int & size_x,
unsigned int & size_y,
unsigned int & angle_quantization,
SearchInfo & search_info );
/**
* @brief Compute the SE2 distance heuristic
* @param lookup_table_dim Size, in costmap pixels, of the
* each lookup table dimension to populate
* @param motion_model Motion model to use for state space
* @param dim_3_size Number of quantization bins for caching
* @param search_info Info containing minimum radius to use
*/
343 static void precomputeDistanceHeuristic(
const float & lookup_table_dim,
const MotionModel & motion_model,
const unsigned int & dim_3_size,
const SearchInfo & search_info );
/**
* @brief Compute the wavefront heuristic
* @param costmap Costmap to use
* @param goal_coords Coordinates to start heuristic expansion at
*/
354 static void resetObstacleHeuristic(
nav2_costmap_2d::Costmap2D * costmap,
const unsigned int & start_x, const unsigned int & start_y,
const unsigned int & goal_x, const unsigned int & goal_y )
{
// State Lattice and Hybrid-A* share this heuristics
NodeHybrid::resetObstacleHeuristic( costmap, start_x, start_y, goal_x, goal_y );
}
/**
* @brief Compute the Obstacle heuristic
* @param node_coords Coordinates to get heuristic at
* @param goal_coords Coordinates to compute heuristic to
* @return heuristic Heuristic value
*/
369 static float getObstacleHeuristic(
const Coordinates & node_coords,
const Coordinates & goal_coords,
const double & cost_penalty )
{
return NodeHybrid::getObstacleHeuristic( node_coords, goal_coords, cost_penalty );
}
/**
* @brief Compute the Distance heuristic
* @param node_coords Coordinates to get heuristic at
* @param goal_coords Coordinates to compute heuristic to
* @param obstacle_heuristic Value of the obstacle heuristic to compute
* additional motion heuristics if required
* @return heuristic Heuristic value
*/
385 static float getDistanceHeuristic(
const Coordinates & node_coords,
const Coordinates & goal_coords,
const float & obstacle_heuristic );
/**
* @brief Retrieve all valid neighbors of a node.
* @param validity_checker Functor for state validity checking
* @param collision_checker Collision checker to use
* @param traverse_unknown If unknown costs are valid to traverse
* @param neighbors Vector of neighbors to be filled
*/
397 void getNeighbors(
std::function<bool( const unsigned int &,
nav2_smac_planner::NodeLattice * & )> & validity_checker,
GridCollisionChecker * collision_checker,
const bool & traverse_unknown,
NodeVector & neighbors );
/**
* @brief Set the starting pose for planning, as a node index
* @param path Reference to a vector of indicies of generated path
* @return whether the path was able to be backtraced
*/
409 bool backtracePath( CoordinateVector & path );
NodeLattice * parent;
Coordinates pose;
static LatticeMotionTable motion_table;
// Dubin / Reeds-Shepp lookup and size for dereferencing
static LookupTable dist_heuristic_lookup_table;
static float size_lookup;
private:
float _cell_cost;
float _accumulated_cost;
unsigned int _index;
bool _was_visited;
MotionPrimitive * _motion_primitive;
bool _backwards;
};
} // namespace nav2_smac_planner
#endif // NAV2_SMAC_PLANNER__NODE_LATTICE_HPP_
// Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef NAV2_SMAC_PLANNER__SMAC_PLANNER_2D_HPP_
#define NAV2_SMAC_PLANNER__SMAC_PLANNER_2D_HPP_
#include <memory>
#include <vector>
#include <string>
#include <mutex>
#include "nav2_smac_planner/a_star.hpp"
#include "nav2_smac_planner/smoother.hpp"
#include "nav2_smac_planner/utils.hpp"
#include "nav2_smac_planner/costmap_downsampler.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "nav2_core/global_planner.hpp"
#include "nav_msgs/msg/path.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/node_utils.hpp"
#include "tf2/utils.h"
#include "rcl_interfaces/msg/set_parameters_result.hpp"
namespace nav2_smac_planner
{
41 class SmacPlanner2D : public nav2_core::GlobalPlanner
{
public:
/**
* @brief constructor
*/
47 SmacPlanner2D( );
/**
* @brief destructor
*/
52 ~SmacPlanner2D( );
/**
* @brief Configuring plugin
* @param parent Lifecycle node pointer
* @param name Name of plugin map
* @param tf Shared ptr of TF2 buffer
* @param costmap_ros Costmap2DROS object
*/
void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros ) override;
/**
* @brief Cleanup lifecycle node
*/
void cleanup( ) override;
/**
* @brief Activate lifecycle node
*/
void activate( ) override;
/**
* @brief Deactivate lifecycle node
*/
void deactivate( ) override;
/**
* @brief Creating a plan from start and goal poses
* @param start Start pose
* @param goal Goal pose
* @return nav2_msgs::Path of the generated path
*/
nav_msgs::msg::Path createPlan(
const geometry_msgs::msg::PoseStamped & start,
const geometry_msgs::msg::PoseStamped & goal ) override;
protected:
/**
* @brief Callback executed when a parameter change is detected
* @param event ParameterEvent message
*/
rcl_interfaces::msg::SetParametersResult
dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters );
std::unique_ptr<AStarAlgorithm<Node2D>> _a_star;
GridCollisionChecker _collision_checker;
std::unique_ptr<Smoother> _smoother;
nav2_costmap_2d::Costmap2D * _costmap;
std::unique_ptr<CostmapDownsampler> _costmap_downsampler;
rclcpp::Clock::SharedPtr _clock;
rclcpp::Logger _logger{rclcpp::get_logger( "SmacPlanner2D" )};
std::string _global_frame, _name;
float _tolerance;
int _downsampling_factor;
bool _downsample_costmap;
rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr _raw_plan_publisher;
double _max_planning_time;
bool _allow_unknown;
int _max_iterations;
int _max_on_approach_iterations;
bool _use_final_approach_orientation;
SearchInfo _search_info;
std::string _motion_model_for_search;
MotionModel _motion_model;
std::mutex _mutex;
rclcpp_lifecycle::LifecycleNode::WeakPtr _node;
// Dynamic parameters handler
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr _dyn_params_handler;
};
} // namespace nav2_smac_planner
#endif // NAV2_SMAC_PLANNER__SMAC_PLANNER_2D_HPP_
// Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef NAV2_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_
#define NAV2_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_
#include <memory>
#include <vector>
#include <string>
#include "nav2_smac_planner/a_star.hpp"
#include "nav2_smac_planner/smoother.hpp"
#include "nav2_smac_planner/utils.hpp"
#include "nav2_smac_planner/costmap_downsampler.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "nav2_core/global_planner.hpp"
#include "nav_msgs/msg/path.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/node_utils.hpp"
#include "tf2/utils.h"
namespace nav2_smac_planner
{
39 class SmacPlannerHybrid : public nav2_core::GlobalPlanner
{
public:
/**
* @brief constructor
*/
45 SmacPlannerHybrid( );
/**
* @brief destructor
*/
50 ~SmacPlannerHybrid( );
/**
* @brief Configuring plugin
* @param parent Lifecycle node pointer
* @param name Name of plugin map
* @param tf Shared ptr of TF2 buffer
* @param costmap_ros Costmap2DROS object
*/
void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros ) override;
/**
* @brief Cleanup lifecycle node
*/
void cleanup( ) override;
/**
* @brief Activate lifecycle node
*/
void activate( ) override;
/**
* @brief Deactivate lifecycle node
*/
void deactivate( ) override;
/**
* @brief Creating a plan from start and goal poses
* @param start Start pose
* @param goal Goal pose
* @return nav2_msgs::Path of the generated path
*/
nav_msgs::msg::Path createPlan(
const geometry_msgs::msg::PoseStamped & start,
const geometry_msgs::msg::PoseStamped & goal ) override;
protected:
/**
* @brief Callback executed when a paramter change is detected
* @param parameters list of changed parameters
*/
rcl_interfaces::msg::SetParametersResult
dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters );
std::unique_ptr<AStarAlgorithm<NodeHybrid>> _a_star;
GridCollisionChecker _collision_checker;
std::unique_ptr<Smoother> _smoother;
rclcpp::Clock::SharedPtr _clock;
rclcpp::Logger _logger{rclcpp::get_logger( "SmacPlannerHybrid" )};
nav2_costmap_2d::Costmap2D * _costmap;
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> _costmap_ros;
std::unique_ptr<CostmapDownsampler> _costmap_downsampler;
std::string _global_frame, _name;
float _lookup_table_dim;
float _tolerance;
bool _downsample_costmap;
int _downsampling_factor;
double _angle_bin_size;
unsigned int _angle_quantizations;
bool _allow_unknown;
int _max_iterations;
SearchInfo _search_info;
double _max_planning_time;
double _lookup_table_size;
double _minimum_turning_radius_global_coords;
std::string _motion_model_for_search;
MotionModel _motion_model;
rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr _raw_plan_publisher;
std::mutex _mutex;
rclcpp_lifecycle::LifecycleNode::WeakPtr _node;
// Dynamic parameters handler
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr _dyn_params_handler;
};
} // namespace nav2_smac_planner
#endif // NAV2_SMAC_PLANNER__SMAC_PLANNER_HYBRID_HPP_
// Copyright ( c ) 2021, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef NAV2_SMAC_PLANNER__SMAC_PLANNER_LATTICE_HPP_
#define NAV2_SMAC_PLANNER__SMAC_PLANNER_LATTICE_HPP_
#include <memory>
#include <vector>
#include <string>
#include "nav2_smac_planner/a_star.hpp"
#include "nav2_smac_planner/smoother.hpp"
#include "nav2_smac_planner/utils.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "nav2_core/global_planner.hpp"
#include "nav_msgs/msg/path.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/node_utils.hpp"
#include "tf2/utils.h"
namespace nav2_smac_planner
{
38 class SmacPlannerLattice : public nav2_core::GlobalPlanner
{
public:
/**
* @brief constructor
*/
44 SmacPlannerLattice( );
/**
* @brief destructor
*/
49 ~SmacPlannerLattice( );
/**
* @brief Configuring plugin
* @param parent Lifecycle node pointer
* @param name Name of plugin map
* @param tf Shared ptr of TF2 buffer
* @param costmap_ros Costmap2DROS object
*/
void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros ) override;
/**
* @brief Cleanup lifecycle node
*/
void cleanup( ) override;
/**
* @brief Activate lifecycle node
*/
void activate( ) override;
/**
* @brief Deactivate lifecycle node
*/
void deactivate( ) override;
/**
* @brief Creating a plan from start and goal poses
* @param start Start pose
* @param goal Goal pose
* @return nav2_msgs::Path of the generated path
*/
nav_msgs::msg::Path createPlan(
const geometry_msgs::msg::PoseStamped & start,
const geometry_msgs::msg::PoseStamped & goal ) override;
protected:
/**
* @brief Callback executed when a paramter change is detected
* @param parameters list of changed parameters
*/
rcl_interfaces::msg::SetParametersResult
dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters );
std::unique_ptr<AStarAlgorithm<NodeLattice>> _a_star;
GridCollisionChecker _collision_checker;
std::unique_ptr<Smoother> _smoother;
rclcpp::Clock::SharedPtr _clock;
rclcpp::Logger _logger{rclcpp::get_logger( "SmacPlannerLattice" )};
nav2_costmap_2d::Costmap2D * _costmap;
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> _costmap_ros;
MotionModel _motion_model;
LatticeMetadata _metadata;
std::string _global_frame, _name;
SearchInfo _search_info;
bool _allow_unknown;
int _max_iterations;
float _tolerance;
rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr _raw_plan_publisher;
double _max_planning_time;
double _lookup_table_size;
std::mutex _mutex;
rclcpp_lifecycle::LifecycleNode::WeakPtr _node;
// Dynamic parameters handler
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr _dyn_params_handler;
};
} // namespace nav2_smac_planner
#endif // NAV2_SMAC_PLANNER__SMAC_PLANNER_LATTICE_HPP_
1 // Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef NAV2_SMAC_PLANNER__TYPES_HPP_
#define NAV2_SMAC_PLANNER__TYPES_HPP_
#include <vector>
#include <utility>
#include <string>
#include <memory>
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "nav2_util/node_utils.hpp"
namespace nav2_smac_planner
{
typedef std::pair<float, unsigned int> NodeHeuristicPair;
/**
* @struct nav2_smac_planner::SearchInfo
* @brief Search properties and penalties
*/
struct SearchInfo
{
float minimum_turning_radius;
float non_straight_penalty;
float change_penalty;
float reverse_penalty;
float cost_penalty;
float retrospective_penalty;
float rotation_penalty;
float analytic_expansion_ratio;
float analytic_expansion_max_length;
std::string lattice_filepath;
bool cache_obstacle_heuristic;
bool allow_reverse_expansion;
};
/**
* @struct nav2_smac_planner::SmootherParams
* @brief Parameters for the smoother
*/
struct SmootherParams
{
/**
* @brief A constructor for nav2_smac_planner::SmootherParams
*/
SmootherParams( )
: holonomic_( false )
{
}
/**
* @brief Get params from ROS parameter
* @param node Ptr to node
* @param name Name of plugin
*/
void get( std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node, const std::string & name )
{
std::string local_name = name + std::string( ".smoother." );
// Smoother params
nav2_util::declare_parameter_if_not_declared(
node, local_name + "tolerance", rclcpp::ParameterValue( 1e-10 ) );
node->get_parameter( local_name + "tolerance", tolerance_ );
nav2_util::declare_parameter_if_not_declared(
node, local_name + "max_iterations", rclcpp::ParameterValue( 1000 ) );
node->get_parameter( local_name + "max_iterations", max_its_ );
nav2_util::declare_parameter_if_not_declared(
node, local_name + "w_data", rclcpp::ParameterValue( 0.2 ) );
node->get_parameter( local_name + "w_data", w_data_ );
nav2_util::declare_parameter_if_not_declared(
node, local_name + "w_smooth", rclcpp::ParameterValue( 0.3 ) );
node->get_parameter( local_name + "w_smooth", w_smooth_ );
nav2_util::declare_parameter_if_not_declared(
node, local_name + "do_refinement", rclcpp::ParameterValue( true ) );
node->get_parameter( local_name + "do_refinement", do_refinement_ );
}
double tolerance_;
int max_its_;
double w_data_;
double w_smooth_;
bool holonomic_;
bool do_refinement_;
};
/**
* @struct nav2_smac_planner::MotionPose
* @brief A struct for poses in motion primitives
*/
struct MotionPose
{
/**
* @brief A constructor for nav2_smac_planner::MotionPose
*/
MotionPose( ) {}
/**
* @brief A constructor for nav2_smac_planner::MotionPose
* @param x X pose
* @param y Y pose
* @param theta Angle of pose
*/
MotionPose( const float & x, const float & y, const float & theta )
: _x( x ), _y( y ), _theta( theta )
{}
MotionPose operator-( const MotionPose & p2 )
{
return MotionPose( this->_x - p2._x, this->_y - p2._y, this->_theta - p2._theta );
}
float _x;
float _y;
float _theta;
};
typedef std::vector<MotionPose> MotionPoses;
/**
* @struct nav2_smac_planner::LatticeMetadata
* @brief A struct of all lattice metadata
*/
struct LatticeMetadata
{
float min_turning_radius;
float grid_resolution;
unsigned int number_of_headings;
std::vector<float> heading_angles;
unsigned int number_of_trajectories;
std::string motion_model;
};
/**
* @struct nav2_smac_planner::MotionPrimitive
* @brief A struct of all motion primitive data
*/
struct MotionPrimitive
{
unsigned int trajectory_id;
float start_angle;
float end_angle;
float turning_radius;
float trajectory_length;
float arc_length;
float straight_length;
bool left_turn;
MotionPoses poses;
};
typedef std::vector<MotionPrimitive> MotionPrimitives;
typedef std::vector<MotionPrimitive *> MotionPrimitivePtrs;
} // namespace nav2_smac_planner
#endif // NAV2_SMAC_PLANNER__TYPES_HPP_
1 // Copyright ( c ) 2021, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef NAV2_SMAC_PLANNER__UTILS_HPP_
#define NAV2_SMAC_PLANNER__UTILS_HPP_
#include <vector>
#include <memory>
#include "Eigen/Core"
#include "geometry_msgs/msg/quaternion.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "tf2/utils.h"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav2_costmap_2d/inflation_layer.hpp"
namespace nav2_smac_planner
{
/**
* @brief Create an Eigen Vector2D of world poses from continuous map coords
* @param mx float of map X coordinate
* @param my float of map Y coordinate
* @param costmap Costmap pointer
* @return Eigen::Vector2d eigen vector of the generated path
*/
38 inline geometry_msgs::msg::Pose getWorldCoords(
const float & mx, const float & my, const nav2_costmap_2d::Costmap2D * costmap )
{
geometry_msgs::msg::Pose msg;
msg.position.x =
static_cast<float>( costmap->getOriginX( ) ) + ( mx + 0.5 ) * costmap->getResolution( );
msg.position.y =
static_cast<float>( costmap->getOriginY( ) ) + ( my + 0.5 ) * costmap->getResolution( );
return msg;
}
/**
* @brief Create quaternion from radians
* @param theta continuous bin coordinates angle
* @return quaternion orientation in map frame
*/
54 inline geometry_msgs::msg::Quaternion getWorldOrientation(
const float & theta )
{
// theta is in radians already
tf2::Quaternion q;
q.setEuler( 0.0, 0.0, theta );
return tf2::toMsg( q );
}
/**
* @brief Find the min cost of the inflation decay function for which the robot MAY be
* in collision in any orientation
* @param costmap Costmap2DROS to get minimum inscribed cost ( e.g. 128 in inflation layer documentation )
* @return double circumscribed cost, any higher than this and need to do full footprint collision checking
* since some element of the robot could be in collision
*/
70 inline double findCircumscribedCost( std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap )
{
double result = -1.0;
bool inflation_layer_found = false;
std::vector<std::shared_ptr<nav2_costmap_2d::Layer>>::iterator layer;
// check if the costmap has an inflation layer
for ( layer = costmap->getLayeredCostmap( )->getPlugins( )->begin( );
layer != costmap->getLayeredCostmap( )->getPlugins( )->end( );
++layer )
{
std::shared_ptr<nav2_costmap_2d::InflationLayer> inflation_layer =
std::dynamic_pointer_cast<nav2_costmap_2d::InflationLayer>( *layer );
if ( !inflation_layer ) {
continue;
}
inflation_layer_found = true;
double circum_radius = costmap->getLayeredCostmap( )->getCircumscribedRadius( );
double resolution = costmap->getCostmap( )->getResolution( );
result = static_cast<double>( inflation_layer->computeCost( circum_radius / resolution ) );
}
if ( !inflation_layer_found ) {
RCLCPP_WARN(
rclcpp::get_logger( "computeCircumscribedCost" ),
"No inflation layer found in costmap configuration. "
"If this is an SE2-collision checking plugin, it cannot use costmap potential "
"field to speed up collision checking by only checking the full footprint "
"when robot is within possibly-inscribed radius of an obstacle. This may "
"significantly slow down planning times!" );
}
return result;
}
/**
* @brief convert json to lattice metadata
* @param[in] json json object
* @param[out] lattice meta data
*/
111 inline void fromJsonToMetaData( const nlohmann::json & json, LatticeMetadata & lattice_metadata )
{
json.at( "turning_radius" ).get_to( lattice_metadata.min_turning_radius );
json.at( "grid_resolution" ).get_to( lattice_metadata.grid_resolution );
json.at( "num_of_headings" ).get_to( lattice_metadata.number_of_headings );
json.at( "heading_angles" ).get_to( lattice_metadata.heading_angles );
json.at( "number_of_trajectories" ).get_to( lattice_metadata.number_of_trajectories );
json.at( "motion_model" ).get_to( lattice_metadata.motion_model );
}
/**
* @brief convert json to pose
* @param[in] json json object
* @param[out] pose
*/
126 inline void fromJsonToPose( const nlohmann::json & json, MotionPose & pose )
{
pose._x = json[0];
pose._y = json[1];
pose._theta = json[2];
}
/**
* @brief convert json to motion primitive
* @param[in] json json object
* @param[out] motion primitive
*/
138 inline void fromJsonToMotionPrimitive(
const nlohmann::json & json, MotionPrimitive & motion_primitive )
{
json.at( "trajectory_id" ).get_to( motion_primitive.trajectory_id );
json.at( "start_angle_index" ).get_to( motion_primitive.start_angle );
json.at( "end_angle_index" ).get_to( motion_primitive.end_angle );
json.at( "trajectory_radius" ).get_to( motion_primitive.turning_radius );
json.at( "trajectory_length" ).get_to( motion_primitive.trajectory_length );
json.at( "arc_length" ).get_to( motion_primitive.arc_length );
json.at( "straight_length" ).get_to( motion_primitive.straight_length );
json.at( "left_turn" ).get_to( motion_primitive.left_turn );
for ( unsigned int i = 0; i < json["poses"].size( ); i++ ) {
MotionPose pose;
fromJsonToPose( json["poses"][i], pose );
motion_primitive.poses.push_back( pose );
}
}
} // namespace nav2_smac_planner
#endif // NAV2_SMAC_PLANNER__UTILS_HPP_
// Copyright ( c ) 2020, Samsung Research America
// Copyright ( c ) 2020, Applied Electric Vehicles Pty Ltd
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <omp.h>
#include <cmath>
#include <stdexcept>
#include <memory>
#include <algorithm>
#include <limits>
#include <type_traits>
#include <chrono>
#include <thread>
#include <utility>
#include <vector>
#include "nav2_smac_planner/a_star.hpp"
using namespace std::chrono; // NOLINT
namespace nav2_smac_planner
{
template<typename NodeT>
35 AStarAlgorithm<NodeT>::AStarAlgorithm(
36 const MotionModel & motion_model,
const SearchInfo & search_info )
: _traverse_unknown( true ),
_max_iterations( 0 ),
_max_planning_time( 0 ),
_x_size( 0 ),
_y_size( 0 ),
_search_info( search_info ),
_goal_coordinates( Coordinates( ) ),
_start( nullptr ),
_goal( nullptr ),
_motion_model( motion_model )
{
_graph.reserve( 100000 );
}
template<typename NodeT>
53 AStarAlgorithm<NodeT>::~AStarAlgorithm( )
{
}
template<typename NodeT>
58 void AStarAlgorithm<NodeT>::initialize(
59 const bool & allow_unknown,
int & max_iterations,
const int & max_on_approach_iterations,
const double & max_planning_time,
const float & lookup_table_size,
const unsigned int & dim_3_size )
{
_traverse_unknown = allow_unknown;
_max_iterations = max_iterations;
_max_on_approach_iterations = max_on_approach_iterations;
_max_planning_time = max_planning_time;
NodeT::precomputeDistanceHeuristic( lookup_table_size, _motion_model, dim_3_size, _search_info );
_dim3_size = dim_3_size;
_expander = std::make_unique<AnalyticExpansion<NodeT>>(
_motion_model, _search_info, _traverse_unknown, _dim3_size );
}
template<>
77 void AStarAlgorithm<Node2D>::initialize(
78 const bool & allow_unknown,
int & max_iterations,
const int & max_on_approach_iterations,
const double & max_planning_time,
const float & /*lookup_table_size*/,
const unsigned int & dim_3_size )
{
_traverse_unknown = allow_unknown;
_max_iterations = max_iterations;
_max_on_approach_iterations = max_on_approach_iterations;
_max_planning_time = max_planning_time;
if ( dim_3_size != 1 ) {
throw std::runtime_error( "Node type Node2D cannot be given non-1 dim 3 quantization." );
}
_dim3_size = dim_3_size;
_expander = std::make_unique<AnalyticExpansion<Node2D>>(
_motion_model, _search_info, _traverse_unknown, _dim3_size );
}
template<typename NodeT>
99 void AStarAlgorithm<NodeT>::setCollisionChecker( GridCollisionChecker * collision_checker )
{
_collision_checker = collision_checker;
_costmap = collision_checker->getCostmap( );
unsigned int x_size = _costmap->getSizeInCellsX( );
unsigned int y_size = _costmap->getSizeInCellsY( );
clearGraph( );
if ( getSizeX( ) != x_size || getSizeY( ) != y_size ) {
_x_size = x_size;
_y_size = y_size;
NodeT::initMotionModel( _motion_model, _x_size, _y_size, _dim3_size, _search_info );
}
_expander->setCollisionChecker( collision_checker );
}
template<typename NodeT>
typename AStarAlgorithm<NodeT>::NodePtr AStarAlgorithm<NodeT>::addToGraph(
const unsigned int & index )
{
// Emplace will only create a new object if it doesn't already exist.
// If an element exists, it will return the existing object, not create a new one.
122 return &( _graph.emplace( index, NodeT( index ) ).first->second );
}
template<>
void AStarAlgorithm<Node2D>::setStart(
const unsigned int & mx,
const unsigned int & my,
const unsigned int & dim_3 )
{
if ( dim_3 != 0 ) {
throw std::runtime_error( "Node type Node2D cannot be given non-zero starting dim 3." );
}
_start = addToGraph( Node2D::getIndex( mx, my, getSizeX( ) ) );
}
template<typename NodeT>
void AStarAlgorithm<NodeT>::setStart(
const unsigned int & mx,
const unsigned int & my,
const unsigned int & dim_3 )
{
_start = addToGraph( NodeT::getIndex( mx, my, dim_3 ) );
_start->setPose(
Coordinates(
static_cast<float>( mx ),
static_cast<float>( my ),
static_cast<float>( dim_3 ) ) );
}
template<>
void AStarAlgorithm<Node2D>::setGoal(
const unsigned int & mx,
const unsigned int & my,
const unsigned int & dim_3 )
{
if ( dim_3 != 0 ) {
throw std::runtime_error( "Node type Node2D cannot be given non-zero goal dim 3." );
}
_goal = addToGraph( Node2D::getIndex( mx, my, getSizeX( ) ) );
_goal_coordinates = Node2D::Coordinates( mx, my );
}
template<typename NodeT>
void AStarAlgorithm<NodeT>::setGoal(
const unsigned int & mx,
const unsigned int & my,
const unsigned int & dim_3 )
{
_goal = addToGraph( NodeT::getIndex( mx, my, dim_3 ) );
typename NodeT::Coordinates goal_coords(
static_cast<float>( mx ),
static_cast<float>( my ),
static_cast<float>( dim_3 ) );
if ( !_search_info.cache_obstacle_heuristic || goal_coords != _goal_coordinates ) {
if ( !_start ) {
throw std::runtime_error( "Start must be set before goal." );
}
NodeT::resetObstacleHeuristic( _costmap, _start->pose.x, _start->pose.y, mx, my );
}
_goal_coordinates = goal_coords;
_goal->setPose( _goal_coordinates );
}
template<typename NodeT>
bool AStarAlgorithm<NodeT>::areInputsValid( )
{
// Check if graph was filled in
if ( _graph.empty( ) ) {
throw std::runtime_error( "Failed to compute path, no costmap given." );
}
// Check if points were filled in
if ( !_start || !_goal ) {
throw std::runtime_error( "Failed to compute path, no valid start or goal given." );
}
// Check if ending point is valid
if ( getToleranceHeuristic( ) < 0.001 &&
!_goal->isNodeValid( _traverse_unknown, _collision_checker ) )
{
throw std::runtime_error( "Failed to compute path, goal is occupied with no tolerance." );
}
// Check if starting point is valid
if ( !_start->isNodeValid( _traverse_unknown, _collision_checker ) ) {
throw std::runtime_error( "Starting point in lethal space! Cannot create feasible plan." );
}
return true;
}
template<typename NodeT>
bool AStarAlgorithm<NodeT>::createPath(
CoordinateVector & path, int & iterations,
const float & tolerance )
{
steady_clock::time_point start_time = steady_clock::now( );
_tolerance = tolerance;
_best_heuristic_node = {std::numeric_limits<float>::max( ), 0};
clearQueue( );
if ( !areInputsValid( ) ) {
return false;
}
// 0 ) Add starting point to the open set
addNode( 0.0, getStart( ) );
getStart( )->setAccumulatedCost( 0.0 );
// Optimization: preallocate all variables
NodePtr current_node = nullptr;
NodePtr neighbor = nullptr;
NodePtr expansion_result = nullptr;
float g_cost = 0.0;
NodeVector neighbors;
int approach_iterations = 0;
NeighborIterator neighbor_iterator;
int analytic_iterations = 0;
int closest_distance = std::numeric_limits<int>::max( );
// Given an index, return a node ptr reference if its collision-free and valid
const unsigned int max_index = getSizeX( ) * getSizeY( ) * getSizeDim3( );
NodeGetter neighborGetter =
[&, this]( const unsigned int & index, NodePtr & neighbor_rtn ) -> bool
{
if ( index >= max_index ) {
return false;
}
neighbor_rtn = addToGraph( index );
return true;
};
while ( iterations < getMaxIterations( ) && !_queue.empty( ) ) {
// Check for planning timeout only on every Nth iteration
if ( iterations % _timing_interval == 0 ) {
std::chrono::duration<double> planning_duration =
std::chrono::duration_cast<std::chrono::duration<double>>( steady_clock::now( ) - start_time );
if ( static_cast<double>( planning_duration.count( ) ) >= _max_planning_time ) {
return false;
}
}
// 1 ) Pick Nbest from O s.t. min( f( Nbest ) ), remove from queue
current_node = getNextNode( );
// We allow for nodes to be queued multiple times in case
// shorter paths result in it, but we can visit only once
if ( current_node->wasVisited( ) ) {
continue;
}
iterations++;
// 2 ) Mark Nbest as visited
current_node->visited( );
// 2.1 ) Use an analytic expansion ( if available ) to generate a path
expansion_result = nullptr;
expansion_result = _expander->tryAnalyticExpansion(
current_node, getGoal( ), neighborGetter, analytic_iterations, closest_distance );
if ( expansion_result != nullptr ) {
current_node = expansion_result;
}
// 3 ) Check if we're at the goal, backtrace if required
if ( isGoal( current_node ) ) {
return current_node->backtracePath( path );
} else if ( _best_heuristic_node.first < getToleranceHeuristic( ) ) {
// Optimization: Let us find when in tolerance and refine within reason
approach_iterations++;
if ( approach_iterations >= getOnApproachMaxIterations( ) ) {
return _graph.at( _best_heuristic_node.second ).backtracePath( path );
}
}
// 4 ) Expand neighbors of Nbest not visited
neighbors.clear( );
current_node->getNeighbors( neighborGetter, _collision_checker, _traverse_unknown, neighbors );
for ( neighbor_iterator = neighbors.begin( );
neighbor_iterator != neighbors.end( ); ++neighbor_iterator )
{
neighbor = *neighbor_iterator;
// 4.1 ) Compute the cost to go to this node
g_cost = current_node->getAccumulatedCost( ) + current_node->getTraversalCost( neighbor );
// 4.2 ) If this is a lower cost than prior, we set this as the new cost and new approach
if ( g_cost < neighbor->getAccumulatedCost( ) ) {
neighbor->setAccumulatedCost( g_cost );
neighbor->parent = current_node;
// 4.3 ) Add to queue with heuristic cost
addNode( g_cost + getHeuristicCost( neighbor ), neighbor );
}
}
}
return false;
}
template<typename NodeT>
bool AStarAlgorithm<NodeT>::isGoal( NodePtr & node )
{
return node == getGoal( );
}
template<typename NodeT>
typename AStarAlgorithm<NodeT>::NodePtr & AStarAlgorithm<NodeT>::getStart( )
{
return _start;
}
template<typename NodeT>
typename AStarAlgorithm<NodeT>::NodePtr & AStarAlgorithm<NodeT>::getGoal( )
{
return _goal;
}
template<typename NodeT>
typename AStarAlgorithm<NodeT>::NodePtr AStarAlgorithm<NodeT>::getNextNode( )
{
NodeBasic<NodeT> node = _queue.top( ).second;
_queue.pop( );
node.processSearchNode( );
return node.graph_node_ptr;
}
template<typename NodeT>
void AStarAlgorithm<NodeT>::addNode( const float & cost, NodePtr & node )
{
NodeBasic<NodeT> queued_node( node->getIndex( ) );
queued_node.populateSearchNode( node );
_queue.emplace( cost, queued_node );
}
template<typename NodeT>
float AStarAlgorithm<NodeT>::getHeuristicCost( const NodePtr & node )
{
const Coordinates node_coords =
NodeT::getCoords( node->getIndex( ), getSizeX( ), getSizeDim3( ) );
float heuristic = NodeT::getHeuristicCost(
node_coords, _goal_coordinates, _costmap );
if ( heuristic < _best_heuristic_node.first ) {
_best_heuristic_node = {heuristic, node->getIndex( )};
}
return heuristic;
}
template<typename NodeT>
void AStarAlgorithm<NodeT>::clearQueue( )
{
NodeQueue q;
std::swap( _queue, q );
}
template<typename NodeT>
void AStarAlgorithm<NodeT>::clearGraph( )
{
Graph g;
std::swap( _graph, g );
_graph.reserve( 100000 );
}
template<typename NodeT>
int & AStarAlgorithm<NodeT>::getMaxIterations( )
{
return _max_iterations;
}
template<typename NodeT>
int & AStarAlgorithm<NodeT>::getOnApproachMaxIterations( )
{
return _max_on_approach_iterations;
}
template<typename NodeT>
float & AStarAlgorithm<NodeT>::getToleranceHeuristic( )
{
return _tolerance;
}
template<typename NodeT>
unsigned int & AStarAlgorithm<NodeT>::getSizeX( )
{
return _x_size;
}
template<typename NodeT>
unsigned int & AStarAlgorithm<NodeT>::getSizeY( )
{
return _y_size;
}
template<typename NodeT>
unsigned int & AStarAlgorithm<NodeT>::getSizeDim3( )
{
return _dim3_size;
}
// Instantiate algorithm for the supported template types
template class AStarAlgorithm<Node2D>;
template class AStarAlgorithm<NodeHybrid>;
template class AStarAlgorithm<NodeLattice>;
} // namespace nav2_smac_planner
// Copyright ( c ) 2021, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <ompl/base/ScopedState.h>
#include <ompl/base/spaces/DubinsStateSpace.h>
#include <ompl/base/spaces/ReedsSheppStateSpace.h>
#include <algorithm>
#include <vector>
#include <memory>
#include "nav2_smac_planner/analytic_expansion.hpp"
namespace nav2_smac_planner
{
template<typename NodeT>
29 AnalyticExpansion<NodeT>::AnalyticExpansion(
30 const MotionModel & motion_model,
const SearchInfo & search_info,
32 const bool & traverse_unknown,
const unsigned int & dim_3_size )
: _motion_model( motion_model ),
_search_info( search_info ),
_traverse_unknown( traverse_unknown ),
_dim_3_size( dim_3_size ),
_collision_checker( nullptr )
{
}
template<typename NodeT>
43 void AnalyticExpansion<NodeT>::setCollisionChecker(
44 GridCollisionChecker * collision_checker )
{
_collision_checker = collision_checker;
}
template<typename NodeT>
typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalyticExpansion(
const NodePtr & current_node, const NodePtr & goal_node,
const NodeGetter & getter, int & analytic_iterations,
int & closest_distance )
{
// This must be a valid motion model for analytic expansion to be attempted
56 if ( _motion_model == MotionModel::DUBIN || _motion_model == MotionModel::REEDS_SHEPP ||
_motion_model == MotionModel::STATE_LATTICE )
{
// See if we are closer and should be expanding more often
auto costmap = _collision_checker->getCostmap( );
const Coordinates node_coords =
NodeT::getCoords( current_node->getIndex( ), costmap->getSizeInCellsX( ), _dim_3_size );
closest_distance = std::min(
closest_distance,
static_cast<int>( NodeT::getHeuristicCost( node_coords, goal_node->pose, costmap ) ) );
// We want to expand at a rate of d/expansion_ratio,
// but check to see if we are so close that we would be expanding every iteration
// If so, limit it to the expansion ratio ( rounded up )
int desired_iterations = std::max(
static_cast<int>( closest_distance / _search_info.analytic_expansion_ratio ),
static_cast<int>( std::ceil( _search_info.analytic_expansion_ratio ) ) );
// If we are closer now, we should update the target number of iterations to go
analytic_iterations =
std::min( analytic_iterations, desired_iterations );
// Always run the expansion on the first run in case there is a
// trivial path to be found
if ( analytic_iterations <= 0 ) {
// Reset the counter and try the analytic path expansion
analytic_iterations = desired_iterations;
AnalyticExpansionNodes analytic_nodes = getAnalyticPath( current_node, goal_node, getter );
if ( !analytic_nodes.empty( ) ) {
// If we have a valid path, attempt to refine it
NodePtr node = current_node;
NodePtr test_node = current_node;
AnalyticExpansionNodes refined_analytic_nodes;
for ( int i = 0; i < 8; i++ ) {
// Attempt to create better paths in 5 node increments, need to make sure
// they exist for each in order to do so ( maximum of 40 points back ).
if ( test_node->parent && test_node->parent->parent && test_node->parent->parent->parent &&
test_node->parent->parent->parent->parent &&
test_node->parent->parent->parent->parent->parent )
{
test_node = test_node->parent->parent->parent->parent->parent;
refined_analytic_nodes = getAnalyticPath( test_node, goal_node, getter );
if ( refined_analytic_nodes.empty( ) ) {
break;
}
analytic_nodes = refined_analytic_nodes;
node = test_node;
} else {
break;
}
}
return setAnalyticPath( node, goal_node, analytic_nodes );
}
}
analytic_iterations--;
}
// No valid motion model - return nullptr
116 return NodePtr( nullptr );
}
template<typename NodeT>
typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<NodeT>::getAnalyticPath(
const NodePtr & node,
const NodePtr & goal,
const NodeGetter & node_getter )
{
125 static ompl::base::ScopedState<> from( node->motion_table.state_space ), to(
node->motion_table.state_space ), s( node->motion_table.state_space );
127 from[0] = node->pose.x;
128 from[1] = node->pose.y;
from[2] = node->motion_table.getAngleFromBin( node->pose.theta );
to[0] = goal->pose.x;
to[1] = goal->pose.y;
to[2] = node->motion_table.getAngleFromBin( goal->pose.theta );
float d = node->motion_table.state_space->distance( from( ), to( ) );
// If the length is too far, exit. This prevents unsafe shortcutting of paths
// into higher cost areas far out from the goal itself, let search to the work of getting
// close before the analytic expansion brings it home. This should never be smaller than
// 4-5x the minimum turning radius being used, or planning times will begin to spike.
if ( d > _search_info.analytic_expansion_max_length ) {
return AnalyticExpansionNodes( );
}
// A move of sqrt( 2 ) is guaranteed to be in a new cell
static const float sqrt_2 = std::sqrt( 2. );
unsigned int num_intervals = std::floor( d / sqrt_2 );
AnalyticExpansionNodes possible_nodes;
// When "from" and "to" are zero or one cell away,
// num_intervals == 0
possible_nodes.reserve( num_intervals ); // We won't store this node or the goal
std::vector<double> reals;
double theta;
// Pre-allocate
NodePtr prev( node );
unsigned int index = 0;
NodePtr next( nullptr );
float angle = 0.0;
Coordinates proposed_coordinates;
bool failure = false;
// Check intermediary poses ( non-goal, non-start )
for ( float i = 1; i < num_intervals; i++ ) {
node->motion_table.state_space->interpolate( from( ), to( ), i / num_intervals, s( ) );
reals = s.reals( );
// Make sure in range [0, 2PI )
theta = ( reals[2] < 0.0 ) ? ( reals[2] + 2.0 * M_PI ) : reals[2];
theta = ( theta > 2.0 * M_PI ) ? ( theta - 2.0 * M_PI ) : theta;
angle = node->motion_table.getClosestAngularBin( theta );
// Turn the pose into a node, and check if it is valid
index = NodeT::getIndex(
static_cast<unsigned int>( reals[0] ),
static_cast<unsigned int>( reals[1] ),
static_cast<unsigned int>( angle ) );
// Get the node from the graph
if ( node_getter( index, next ) ) {
Coordinates initial_node_coords = next->pose;
proposed_coordinates = {static_cast<float>( reals[0] ), static_cast<float>( reals[1] ), angle};
next->setPose( proposed_coordinates );
if ( next->isNodeValid( _traverse_unknown, _collision_checker ) && next != prev ) {
// Save the node, and its previous coordinates in case we need to abort
possible_nodes.emplace_back( next, initial_node_coords, proposed_coordinates );
prev = next;
} else {
// Abort
next->setPose( initial_node_coords );
failure = true;
break;
}
} else {
// Abort
failure = true;
break;
}
}
// Reset to initial poses to not impact future searches
for ( const auto & node_pose : possible_nodes ) {
const auto & n = node_pose.node;
n->setPose( node_pose.initial_coords );
}
if ( failure ) {
return AnalyticExpansionNodes( );
}
return possible_nodes;
}
template<typename NodeT>
typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::setAnalyticPath(
const NodePtr & node,
const NodePtr & goal_node,
const AnalyticExpansionNodes & expanded_nodes )
{
_detached_nodes.clear( );
// Legitimate final path - set the parent relationships, states, and poses
NodePtr prev = node;
for ( const auto & node_pose : expanded_nodes ) {
auto n = node_pose.node;
cleanNode( n );
if ( n->getIndex( ) != goal_node->getIndex( ) ) {
if ( n->wasVisited( ) ) {
_detached_nodes.push_back( std::make_unique<NodeT>( -1 ) );
n = _detached_nodes.back( ).get( );
}
n->parent = prev;
n->pose = node_pose.proposed_coords;
n->visited( );
prev = n;
}
}
if ( goal_node != prev ) {
goal_node->parent = prev;
cleanNode( goal_node );
goal_node->visited( );
}
return goal_node;
}
template<>
void AnalyticExpansion<NodeLattice>::cleanNode( const NodePtr & node )
{
node->setMotionPrimitive( nullptr );
}
template<typename NodeT>
void AnalyticExpansion<NodeT>::cleanNode( const NodePtr & /*expanded_nodes*/ )
{
}
template<>
typename AnalyticExpansion<Node2D>::AnalyticExpansionNodes AnalyticExpansion<Node2D>::
getAnalyticPath(
const NodePtr & node,
const NodePtr & goal,
const NodeGetter & node_getter )
{
return AnalyticExpansionNodes( );
}
template<>
typename AnalyticExpansion<Node2D>::NodePtr AnalyticExpansion<Node2D>::setAnalyticPath(
const NodePtr & node,
const NodePtr & goal_node,
const AnalyticExpansionNodes & expanded_nodes )
{
return NodePtr( nullptr );
}
template<>
typename AnalyticExpansion<Node2D>::NodePtr AnalyticExpansion<Node2D>::tryAnalyticExpansion(
const NodePtr & current_node, const NodePtr & goal_node,
const NodeGetter & getter, int & analytic_iterations,
int & closest_distance )
{
return NodePtr( nullptr );
}
template class AnalyticExpansion<Node2D>;
template class AnalyticExpansion<NodeHybrid>;
template class AnalyticExpansion<NodeLattice>;
} // namespace nav2_smac_planner
1 // Copyright ( c ) 2021, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include "nav2_smac_planner/collision_checker.hpp"
namespace nav2_smac_planner
{
20 GridCollisionChecker::GridCollisionChecker(
21 nav2_costmap_2d::Costmap2D * costmap,
unsigned int num_quantizations )
: FootprintCollisionChecker( costmap )
{
// Convert number of regular bins into angles
float bin_size = 2 * M_PI / static_cast<float>( num_quantizations );
angles_.reserve( num_quantizations );
for ( unsigned int i = 0; i != num_quantizations; i++ ) {
angles_.push_back( bin_size * i );
}
}
// GridCollisionChecker::GridCollisionChecker(
// nav2_costmap_2d::Costmap2D * costmap,
// std::vector<float> & angles )
// : FootprintCollisionChecker( costmap ),
// angles_( angles )
// {
// }
41 void GridCollisionChecker::setFootprint(
42 const nav2_costmap_2d::Footprint & footprint,
43 const bool & radius,
const double & possible_inscribed_cost )
{
possible_inscribed_cost_ = possible_inscribed_cost;
footprint_is_radius_ = radius;
// Use radius, no caching required
if ( radius ) {
return;
}
// No change, no updates required
if ( footprint == unoriented_footprint_ ) {
return;
}
oriented_footprints_.reserve( angles_.size( ) );
double sin_th, cos_th;
geometry_msgs::msg::Point new_pt;
const unsigned int footprint_size = footprint.size( );
// Precompute the orientation bins for checking to use
for ( unsigned int i = 0; i != angles_.size( ); i++ ) {
sin_th = sin( angles_[i] );
cos_th = cos( angles_[i] );
nav2_costmap_2d::Footprint oriented_footprint;
oriented_footprint.reserve( footprint_size );
for ( unsigned int j = 0; j < footprint_size; j++ ) {
new_pt.x = footprint[j].x * cos_th - footprint[j].y * sin_th;
new_pt.y = footprint[j].x * sin_th + footprint[j].y * cos_th;
oriented_footprint.push_back( new_pt );
}
oriented_footprints_.push_back( oriented_footprint );
}
unoriented_footprint_ = footprint;
}
83 bool GridCollisionChecker::inCollision(
const float & x,
const float & y,
const float & angle_bin,
87 const bool & traverse_unknown )
{
// Check to make sure cell is inside the map
if ( outsideRange( costmap_->getSizeInCellsX( ), x ) ||
outsideRange( costmap_->getSizeInCellsY( ), y ) )
{
return false;
}
// Assumes setFootprint already set
double wx, wy;
costmap_->mapToWorld( static_cast<double>( x ), static_cast<double>( y ), wx, wy );
if ( !footprint_is_radius_ ) {
// if footprint, then we check for the footprint's points, but first see
// if the robot is even potentially in an inscribed collision
footprint_cost_ = costmap_->getCost(
static_cast<unsigned int>( x ), static_cast<unsigned int>( y ) );
if ( footprint_cost_ < possible_inscribed_cost_ ) {
return false;
}
// If its inscribed, in collision, or unknown in the middle,
// no need to even check the footprint, its invalid
if ( footprint_cost_ == UNKNOWN && !traverse_unknown ) {
return true;
}
if ( footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED ) {
return true;
}
// if possible inscribed, need to check actual footprint pose.
// Use precomputed oriented footprints are done on initialization,
// offset by translation value to collision check
geometry_msgs::msg::Point new_pt;
const nav2_costmap_2d::Footprint & oriented_footprint = oriented_footprints_[angle_bin];
nav2_costmap_2d::Footprint current_footprint;
current_footprint.reserve( oriented_footprint.size( ) );
for ( unsigned int i = 0; i < oriented_footprint.size( ); ++i ) {
new_pt.x = wx + oriented_footprint[i].x;
new_pt.y = wy + oriented_footprint[i].y;
current_footprint.push_back( new_pt );
}
footprint_cost_ = footprintCost( current_footprint );
if ( footprint_cost_ == UNKNOWN && traverse_unknown ) {
return false;
}
// if occupied or unknown and not to traverse unknown space
return footprint_cost_ >= OCCUPIED;
} else {
// if radius, then we can check the center of the cost assuming inflation is used
footprint_cost_ = costmap_->getCost(
static_cast<unsigned int>( x ), static_cast<unsigned int>( y ) );
if ( footprint_cost_ == UNKNOWN && traverse_unknown ) {
return false;
}
// if occupied or unknown and not to traverse unknown space
return static_cast<double>( footprint_cost_ ) >= INSCRIBED;
}
}
155 bool GridCollisionChecker::inCollision(
const unsigned int & i,
157 const bool & traverse_unknown )
{
footprint_cost_ = costmap_->getCost( i );
if ( footprint_cost_ == UNKNOWN && traverse_unknown ) {
return false;
}
// if occupied or unknown and not to traverse unknown space
return footprint_cost_ >= INSCRIBED;
}
168 float GridCollisionChecker::getCost( )
{
// Assumes inCollision called prior
return static_cast<float>( footprint_cost_ );
}
174 bool GridCollisionChecker::outsideRange( const unsigned int & max, const float & value )
{
return value < 0.0f || value > max;
}
} // namespace nav2_smac_planner
1 // Copyright ( c ) 2020, Carlos Luis
// Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include "nav2_smac_planner/costmap_downsampler.hpp"
#include <string>
#include <memory>
#include <algorithm>
namespace nav2_smac_planner
{
25 CostmapDownsampler::CostmapDownsampler( )
: _costmap( nullptr ),
_downsampled_costmap( nullptr ),
_downsampled_costmap_pub( nullptr )
{
}
32 CostmapDownsampler::~CostmapDownsampler( )
{
}
36 void CostmapDownsampler::on_configure(
const nav2_util::LifecycleNode::WeakPtr & node,
const std::string & global_frame,
const std::string & topic_name,
nav2_costmap_2d::Costmap2D * const costmap,
const unsigned int & downsampling_factor,
const bool & use_min_cost_neighbor )
{
_costmap = costmap;
_downsampling_factor = downsampling_factor;
_use_min_cost_neighbor = use_min_cost_neighbor;
updateCostmapSize( );
_downsampled_costmap = std::make_unique<nav2_costmap_2d::Costmap2D>(
_downsampled_size_x, _downsampled_size_y, _downsampled_resolution,
_costmap->getOriginX( ), _costmap->getOriginY( ), UNKNOWN );
if ( !node.expired( ) ) {
_downsampled_costmap_pub = std::make_unique<nav2_costmap_2d::Costmap2DPublisher>(
node, _downsampled_costmap.get( ), global_frame, topic_name, false );
}
}
59 void CostmapDownsampler::on_activate( )
{
if ( _downsampled_costmap_pub ) {
_downsampled_costmap_pub->on_activate( );
}
}
66 void CostmapDownsampler::on_deactivate( )
{
if ( _downsampled_costmap_pub ) {
_downsampled_costmap_pub->on_deactivate( );
}
}
73 void CostmapDownsampler::on_cleanup( )
{
_costmap = nullptr;
_downsampled_costmap.reset( );
_downsampled_costmap_pub.reset( );
}
80 nav2_costmap_2d::Costmap2D * CostmapDownsampler::downsample(
const unsigned int & downsampling_factor )
{
_downsampling_factor = downsampling_factor;
updateCostmapSize( );
// Adjust costmap size if needed
if ( _downsampled_costmap->getSizeInCellsX( ) != _downsampled_size_x ||
_downsampled_costmap->getSizeInCellsY( ) != _downsampled_size_y ||
_downsampled_costmap->getResolution( ) != _downsampled_resolution )
{
resizeCostmap( );
}
// Assign costs
for ( unsigned int i = 0; i < _downsampled_size_x; ++i ) {
for ( unsigned int j = 0; j < _downsampled_size_y; ++j ) {
setCostOfCell( i, j );
}
}
if ( _downsampled_costmap_pub ) {
_downsampled_costmap_pub->publishCostmap( );
}
return _downsampled_costmap.get( );
}
107 void CostmapDownsampler::updateCostmapSize( )
{
_size_x = _costmap->getSizeInCellsX( );
_size_y = _costmap->getSizeInCellsY( );
_downsampled_size_x = ceil( static_cast<float>( _size_x ) / _downsampling_factor );
_downsampled_size_y = ceil( static_cast<float>( _size_y ) / _downsampling_factor );
_downsampled_resolution = _downsampling_factor * _costmap->getResolution( );
}
116 void CostmapDownsampler::resizeCostmap( )
{
_downsampled_costmap->resizeMap(
_downsampled_size_x,
_downsampled_size_y,
_downsampled_resolution,
_costmap->getOriginX( ),
_costmap->getOriginY( ) );
}
126 void CostmapDownsampler::setCostOfCell(
const unsigned int & new_mx,
const unsigned int & new_my )
{
unsigned int mx, my;
unsigned char cost = _use_min_cost_neighbor ? 255 : 0;
unsigned int x_offset = new_mx * _downsampling_factor;
unsigned int y_offset = new_my * _downsampling_factor;
for ( unsigned int i = 0; i < _downsampling_factor; ++i ) {
mx = x_offset + i;
if ( mx >= _size_x ) {
continue;
}
for ( unsigned int j = 0; j < _downsampling_factor; ++j ) {
my = y_offset + j;
if ( my >= _size_y ) {
continue;
}
cost = _use_min_cost_neighbor ?
std::min( cost, _costmap->getCost( mx, my ) ) :
std::max( cost, _costmap->getCost( mx, my ) );
}
}
_downsampled_costmap->setCost( new_mx, new_my, cost );
}
} // namespace nav2_smac_planner
// Copyright ( c ) 2020, Samsung Research America
// Copyright ( c ) 2020, Applied Electric Vehicles Pty Ltd
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include "nav2_smac_planner/node_2d.hpp"
#include <vector>
#include <limits>
namespace nav2_smac_planner
{
// defining static member for all instance to share
std::vector<int> Node2D::_neighbors_grid_offsets;
float Node2D::cost_travel_multiplier = 2.0;
28 Node2D::Node2D( const unsigned int index )
: parent( nullptr ),
_cell_cost( std::numeric_limits<float>::quiet_NaN( ) ),
_accumulated_cost( std::numeric_limits<float>::max( ) ),
_index( index ),
_was_visited( false ),
_is_queued( false )
{
}
Node2D::~Node2D( )
{
parent = nullptr;
}
void Node2D::reset( )
{
parent = nullptr;
_cell_cost = std::numeric_limits<float>::quiet_NaN( );
_accumulated_cost = std::numeric_limits<float>::max( );
_was_visited = false;
_is_queued = false;
}
bool Node2D::isNodeValid(
const bool & traverse_unknown,
GridCollisionChecker * collision_checker )
{
if ( collision_checker->inCollision( this->getIndex( ), traverse_unknown ) ) {
return false;
}
_cell_cost = collision_checker->getCost( );
return true;
}
float Node2D::getTraversalCost( const NodePtr & child )
{
float normalized_cost = child->getCost( ) / 252.0;
const Coordinates A = getCoords( child->getIndex( ) );
const Coordinates B = getCoords( this->getIndex( ) );
const float & dx = A.x - B.x;
const float & dy = A.y - B.y;
static float sqrt_2 = sqrt( 2 );
// If a diagonal move, travel cost is sqrt( 2 ) not 1.0.
if ( ( dx * dx + dy * dy ) > 1.05 ) {
return sqrt_2 + cost_travel_multiplier * normalized_cost;
}
return 1.0 + cost_travel_multiplier * normalized_cost;
}
float Node2D::getHeuristicCost(
const Coordinates & node_coords,
const Coordinates & goal_coordinates,
const nav2_costmap_2d::Costmap2D * /*costmap*/ )
{
// Using Moore distance as it more accurately represents the distances
// even a Van Neumann neighborhood robot can navigate.
return fabs( goal_coordinates.x - node_coords.x ) +
fabs( goal_coordinates.y - node_coords.y );
}
void Node2D::initMotionModel(
const MotionModel & neighborhood,
unsigned int & x_size_uint,
unsigned int & /*size_y*/,
unsigned int & /*num_angle_quantization*/,
SearchInfo & search_info )
{
int x_size = static_cast<int>( x_size_uint );
cost_travel_multiplier = search_info.cost_penalty;
switch ( neighborhood ) {
case MotionModel::UNKNOWN:
throw std::runtime_error( "Unknown neighborhood type selected." );
case MotionModel::VON_NEUMANN:
_neighbors_grid_offsets = {-1, +1, -x_size, +x_size};
break;
case MotionModel::MOORE:
_neighbors_grid_offsets = {-1, +1, -x_size, +x_size, -x_size - 1,
-x_size + 1, +x_size - 1, +x_size + 1};
break;
default:
throw std::runtime_error(
"Invalid neighborhood type selected. "
"Von-Neumann and Moore are valid for Node2D." );
}
}
void Node2D::getNeighbors(
std::function<bool( const unsigned int &, nav2_smac_planner::Node2D * & )> & NeighborGetter,
GridCollisionChecker * collision_checker,
const bool & traverse_unknown,
NodeVector & neighbors )
{
// NOTE( stevemacenski ): Irritatingly, the order here matters. If you start in free
// space and then expand 8-connected, the first set of neighbors will be all cost
// 1.0. Then its expansion will all be 2 * 1.0 but now multiple
// nodes are touching that node so the last cell to update the back pointer wins.
// Thusly, the ordering ends with the cardinal directions for both sets such that
// behavior is consistent in large free spaces between them.
// 100 50 0
// 100 50 50
// 100 100 100 where lower-middle '100' is visited with same cost by both bottom '50' nodes
// Therefore, it is valuable to have some low-potential across the entire map
// rather than a small inflation around the obstacles
int index;
NodePtr neighbor;
int node_i = this->getIndex( );
const Coordinates parent = getCoords( this->getIndex( ) );
Coordinates child;
for ( unsigned int i = 0; i != _neighbors_grid_offsets.size( ); ++i ) {
index = node_i + _neighbors_grid_offsets[i];
// Check for wrap around conditions
child = getCoords( index );
if ( fabs( parent.x - child.x ) > 1 || fabs( parent.y - child.y ) > 1 ) {
continue;
}
if ( NeighborGetter( index, neighbor ) ) {
if ( neighbor->isNodeValid( traverse_unknown, collision_checker ) && !neighbor->wasVisited( ) ) {
neighbors.push_back( neighbor );
}
}
}
}
bool Node2D::backtracePath( CoordinateVector & path )
{
if ( !this->parent ) {
return false;
}
NodePtr current_node = this;
while ( current_node->parent ) {
path.push_back(
Node2D::getCoords( current_node->getIndex( ) ) );
current_node = current_node->parent;
}
return path.size( ) > 0;
}
} // namespace nav2_smac_planner
1 // Copyright ( c ) 2021, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include "nav2_smac_planner/node_basic.hpp"
namespace nav2_smac_planner
{
template<typename Node2D>
21 void NodeBasic<Node2D>::processSearchNode( )
{
}
template<>
26 void NodeBasic<NodeHybrid>::processSearchNode( )
{
// We only want to override the node's pose if it has not yet been visited
// to prevent the case that a node has been queued multiple times and
// a new branch is overriding one of lower cost already visited.
if ( !this->graph_node_ptr->wasVisited( ) ) {
this->graph_node_ptr->pose = this->pose;
this->graph_node_ptr->setMotionPrimitiveIndex( this->motion_index );
}
}
template<>
38 void NodeBasic<NodeLattice>::processSearchNode( )
{
// We only want to override the node's pose/primitive if it has not yet been visited
// to prevent the case that a node has been queued multiple times and
// a new branch is overriding one of lower cost already visited.
if ( !this->graph_node_ptr->wasVisited( ) ) {
this->graph_node_ptr->pose = this->pose;
this->graph_node_ptr->setMotionPrimitive( this->prim_ptr );
this->graph_node_ptr->backwards( this->backward );
}
}
template<>
51 void NodeBasic<Node2D>::populateSearchNode( Node2D * & node )
{
this->graph_node_ptr = node;
}
template<>
57 void NodeBasic<NodeHybrid>::populateSearchNode( NodeHybrid * & node )
{
this->pose = node->pose;
this->graph_node_ptr = node;
this->motion_index = node->getMotionPrimitiveIndex( );
}
template<>
65 void NodeBasic<NodeLattice>::populateSearchNode( NodeLattice * & node )
{
this->pose = node->pose;
this->graph_node_ptr = node;
this->prim_ptr = node->getMotionPrimitive( );
this->backward = node->isBackward( );
}
73 template class NodeBasic<Node2D>;
74 template class NodeBasic<NodeHybrid>;
75 template class NodeBasic<NodeLattice>;
} // namespace nav2_smac_planner
// Copyright ( c ) 2020, Samsung Research America
// Copyright ( c ) 2020, Applied Electric Vehicles Pty Ltd
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <math.h>
#include <chrono>
#include <vector>
#include <memory>
#include <algorithm>
#include <queue>
#include <limits>
#include <utility>
#include "ompl/base/ScopedState.h"
#include "ompl/base/spaces/DubinsStateSpace.h"
#include "ompl/base/spaces/ReedsSheppStateSpace.h"
#include "nav2_smac_planner/node_hybrid.hpp"
using namespace std::chrono; // NOLINT
namespace nav2_smac_planner
{
// defining static member for all instance to share
LookupTable NodeHybrid::obstacle_heuristic_lookup_table;
double NodeHybrid::travel_distance_cost = sqrt( 2 );
HybridMotionTable NodeHybrid::motion_table;
float NodeHybrid::size_lookup = 25;
LookupTable NodeHybrid::dist_heuristic_lookup_table;
nav2_costmap_2d::Costmap2D * NodeHybrid::sampled_costmap = nullptr;
CostmapDownsampler NodeHybrid::downsampler;
ObstacleHeuristicQueue NodeHybrid::obstacle_heuristic_queue;
// Each of these tables are the projected motion models through
// time and space applied to the search on the current node in
// continuous map-coordinates ( e.g. not meters but partial map cells )
// Currently, these are set to project *at minimum* into a neighboring
// cell. Though this could be later modified to project a certain
// amount of time or particular distance forward.
// http://planning.cs.uiuc.edu/node821.html
// Model for ackermann style vehicle with minimum radius restriction
void HybridMotionTable::initDubin(
unsigned int & size_x_in,
unsigned int & /*size_y_in*/,
unsigned int & num_angle_quantization_in,
SearchInfo & search_info )
{
size_x = size_x_in;
change_penalty = search_info.change_penalty;
non_straight_penalty = search_info.non_straight_penalty;
cost_penalty = search_info.cost_penalty;
reverse_penalty = search_info.reverse_penalty;
travel_distance_reward = 1.0f - search_info.retrospective_penalty;
// if nothing changed, no need to re-compute primitives
if ( num_angle_quantization_in == num_angle_quantization &&
min_turning_radius == search_info.minimum_turning_radius &&
motion_model == MotionModel::DUBIN )
{
return;
}
num_angle_quantization = num_angle_quantization_in;
num_angle_quantization_float = static_cast<float>( num_angle_quantization );
min_turning_radius = search_info.minimum_turning_radius;
motion_model = MotionModel::DUBIN;
// angle must meet 3 requirements:
// 1 ) be increment of quantized bin size
// 2 ) chord length must be greater than sqrt( 2 ) to leave current cell
// 3 ) maximum curvature must be respected, represented by minimum turning angle
// Thusly:
// On circle of radius minimum turning angle, we need select motion primatives
// with chord length > sqrt( 2 ) and be an increment of our bin size
//
// chord >= sqrt( 2 ) >= 2 * R * sin ( angle / 2 ); where angle / N = quantized bin size
// Thusly: angle <= 2.0 * asin( sqrt( 2 ) / ( 2 * R ) )
float angle = 2.0 * asin( sqrt( 2.0 ) / ( 2 * min_turning_radius ) );
// Now make sure angle is an increment of the quantized bin size
// And since its based on the minimum chord, we need to make sure its always larger
bin_size =
2.0f * static_cast<float>( M_PI ) / static_cast<float>( num_angle_quantization );
float increments;
if ( angle < bin_size ) {
increments = 1.0f;
} else {
// Search dimensions are clean multiples of quantization - this prevents
// paths with loops in them
increments = ceil( angle / bin_size );
}
angle = increments * bin_size;
// find deflections
// If we make a right triangle out of the chord in circle of radius
// min turning angle, we can see that delta X = R * sin ( angle )
float delta_x = min_turning_radius * sin( angle );
// Using that same right triangle, we can see that the complement
// to delta Y is R * cos ( angle ). If we subtract R, we get the actual value
float delta_y = min_turning_radius - ( min_turning_radius * cos( angle ) );
projections.clear( );
projections.reserve( 3 );
projections.emplace_back( hypotf( delta_x, delta_y ), 0.0, 0.0 ); // Forward
projections.emplace_back( delta_x, delta_y, increments ); // Left
projections.emplace_back( delta_x, -delta_y, -increments ); // Right
// Create the correct OMPL state space
state_space = std::make_unique<ompl::base::DubinsStateSpace>( min_turning_radius );
// Precompute projection deltas
delta_xs.resize( projections.size( ) );
delta_ys.resize( projections.size( ) );
trig_values.resize( num_angle_quantization );
for ( unsigned int i = 0; i != projections.size( ); i++ ) {
delta_xs[i].resize( num_angle_quantization );
delta_ys[i].resize( num_angle_quantization );
for ( unsigned int j = 0; j != num_angle_quantization; j++ ) {
double cos_theta = cos( bin_size * j );
double sin_theta = sin( bin_size * j );
if ( i == 0 ) {
// if first iteration, cache the trig values for later
trig_values[j] = {cos_theta, sin_theta};
}
delta_xs[i][j] = projections[i]._x * cos_theta - projections[i]._y * sin_theta;
delta_ys[i][j] = projections[i]._x * sin_theta + projections[i]._y * cos_theta;
}
}
}
// http://planning.cs.uiuc.edu/node822.html
// Same as Dubin model but now reverse is valid
// See notes in Dubin for explanation
void HybridMotionTable::initReedsShepp(
unsigned int & size_x_in,
unsigned int & /*size_y_in*/,
unsigned int & num_angle_quantization_in,
SearchInfo & search_info )
{
size_x = size_x_in;
change_penalty = search_info.change_penalty;
non_straight_penalty = search_info.non_straight_penalty;
cost_penalty = search_info.cost_penalty;
reverse_penalty = search_info.reverse_penalty;
travel_distance_reward = 1.0f - search_info.retrospective_penalty;
// if nothing changed, no need to re-compute primitives
if ( num_angle_quantization_in == num_angle_quantization &&
min_turning_radius == search_info.minimum_turning_radius &&
motion_model == MotionModel::REEDS_SHEPP )
{
return;
}
num_angle_quantization = num_angle_quantization_in;
num_angle_quantization_float = static_cast<float>( num_angle_quantization );
min_turning_radius = search_info.minimum_turning_radius;
motion_model = MotionModel::REEDS_SHEPP;
float angle = 2.0 * asin( sqrt( 2.0 ) / ( 2 * min_turning_radius ) );
bin_size =
2.0f * static_cast<float>( M_PI ) / static_cast<float>( num_angle_quantization );
float increments;
if ( angle < bin_size ) {
increments = 1.0f;
} else {
increments = ceil( angle / bin_size );
}
angle = increments * bin_size;
float delta_x = min_turning_radius * sin( angle );
float delta_y = min_turning_radius - ( min_turning_radius * cos( angle ) );
projections.clear( );
projections.reserve( 6 );
projections.emplace_back( hypotf( delta_x, delta_y ), 0.0, 0.0 ); // Forward
projections.emplace_back( delta_x, delta_y, increments ); // Forward + Left
projections.emplace_back( delta_x, -delta_y, -increments ); // Forward + Right
projections.emplace_back( -hypotf( delta_x, delta_y ), 0.0, 0.0 ); // Backward
projections.emplace_back( -delta_x, delta_y, -increments ); // Backward + Left
projections.emplace_back( -delta_x, -delta_y, increments ); // Backward + Right
// Create the correct OMPL state space
state_space = std::make_unique<ompl::base::ReedsSheppStateSpace>( min_turning_radius );
// Precompute projection deltas
delta_xs.resize( projections.size( ) );
delta_ys.resize( projections.size( ) );
trig_values.resize( num_angle_quantization );
for ( unsigned int i = 0; i != projections.size( ); i++ ) {
delta_xs[i].resize( num_angle_quantization );
delta_ys[i].resize( num_angle_quantization );
for ( unsigned int j = 0; j != num_angle_quantization; j++ ) {
double cos_theta = cos( bin_size * j );
double sin_theta = sin( bin_size * j );
if ( i == 0 ) {
// if first iteration, cache the trig values for later
trig_values[j] = {cos_theta, sin_theta};
}
delta_xs[i][j] = projections[i]._x * cos_theta - projections[i]._y * sin_theta;
delta_ys[i][j] = projections[i]._x * sin_theta + projections[i]._y * cos_theta;
}
}
}
MotionPoses HybridMotionTable::getProjections( const NodeHybrid * node )
{
MotionPoses projection_list;
projection_list.reserve( projections.size( ) );
for ( unsigned int i = 0; i != projections.size( ); i++ ) {
const MotionPose & motion_model = projections[i];
// normalize theta, I know its overkill, but I've been burned before...
const float & node_heading = node->pose.theta;
float new_heading = node_heading + motion_model._theta;
if ( new_heading < 0.0 ) {
new_heading += num_angle_quantization_float;
}
if ( new_heading >= num_angle_quantization_float ) {
new_heading -= num_angle_quantization_float;
}
projection_list.emplace_back(
delta_xs[i][node_heading] + node->pose.x,
delta_ys[i][node_heading] + node->pose.y,
new_heading );
}
return projection_list;
}
unsigned int HybridMotionTable::getClosestAngularBin( const double & theta )
{
return static_cast<unsigned int>( floor( theta / bin_size ) );
}
float HybridMotionTable::getAngleFromBin( const unsigned int & bin_idx )
{
return bin_idx * bin_size;
}
261 NodeHybrid::NodeHybrid( const unsigned int index )
: parent( nullptr ),
263 pose( 0.0f, 0.0f, 0.0f ),
_cell_cost( std::numeric_limits<float>::quiet_NaN( ) ),
_accumulated_cost( std::numeric_limits<float>::max( ) ),
_index( index ),
_was_visited( false ),
_motion_primitive_index( std::numeric_limits<unsigned int>::max( ) )
{
}
272 NodeHybrid::~NodeHybrid( )
{
parent = nullptr;
}
277 void NodeHybrid::reset( )
{
parent = nullptr;
_cell_cost = std::numeric_limits<float>::quiet_NaN( );
_accumulated_cost = std::numeric_limits<float>::max( );
_was_visited = false;
_motion_primitive_index = std::numeric_limits<unsigned int>::max( );
pose.x = 0.0f;
pose.y = 0.0f;
pose.theta = 0.0f;
}
289 bool NodeHybrid::isNodeValid(
290 const bool & traverse_unknown,
291 GridCollisionChecker * collision_checker )
{
if ( collision_checker->inCollision(
this->pose.x, this->pose.y, this->pose.theta /*bin number*/, traverse_unknown ) )
{
return false;
}
_cell_cost = collision_checker->getCost( );
return true;
}
303 float NodeHybrid::getTraversalCost( const NodePtr & child )
{
const float normalized_cost = child->getCost( ) / 252.0;
if ( std::isnan( normalized_cost ) ) {
throw std::runtime_error(
"Node attempted to get traversal "
"cost without a known SE2 collision cost!" );
}
// this is the first node
if ( getMotionPrimitiveIndex( ) == std::numeric_limits<unsigned int>::max( ) ) {
return NodeHybrid::travel_distance_cost;
}
float travel_cost = 0.0;
float travel_cost_raw =
NodeHybrid::travel_distance_cost *
( motion_table.travel_distance_reward + motion_table.cost_penalty * normalized_cost );
if ( child->getMotionPrimitiveIndex( ) == 0 || child->getMotionPrimitiveIndex( ) == 3 ) {
// New motion is a straight motion, no additional costs to be applied
travel_cost = travel_cost_raw;
} else {
if ( getMotionPrimitiveIndex( ) == child->getMotionPrimitiveIndex( ) ) {
// Turning motion but keeps in same direction: encourages to commit to turning if starting it
travel_cost = travel_cost_raw * motion_table.non_straight_penalty;
} else {
// Turning motion and changing direction: penalizes wiggling
travel_cost = travel_cost_raw *
( motion_table.non_straight_penalty + motion_table.change_penalty );
}
}
if ( child->getMotionPrimitiveIndex( ) > 2 ) {
// reverse direction
travel_cost *= motion_table.reverse_penalty;
}
return travel_cost;
}
344 float NodeHybrid::getHeuristicCost(
345 const Coordinates & node_coords,
346 const Coordinates & goal_coords,
347 const nav2_costmap_2d::Costmap2D * /*costmap*/ )
{
const float obstacle_heuristic =
getObstacleHeuristic( node_coords, goal_coords, motion_table.cost_penalty );
const float dist_heuristic = getDistanceHeuristic( node_coords, goal_coords, obstacle_heuristic );
return std::max( obstacle_heuristic, dist_heuristic );
}
355 void NodeHybrid::initMotionModel(
356 const MotionModel & motion_model,
unsigned int & size_x,
unsigned int & size_y,
unsigned int & num_angle_quantization,
SearchInfo & search_info )
{
// find the motion model selected
switch ( motion_model ) {
case MotionModel::DUBIN:
motion_table.initDubin( size_x, size_y, num_angle_quantization, search_info );
break;
case MotionModel::REEDS_SHEPP:
motion_table.initReedsShepp( size_x, size_y, num_angle_quantization, search_info );
break;
default:
throw std::runtime_error(
"Invalid motion model for Hybrid A*. Please select between"
" Dubin ( Ackermann forward only ), "
" Reeds-Shepp ( Ackermann forward and back )." );
}
travel_distance_cost = motion_table.projections[0]._x;
}
380 inline float distanceHeuristic2D(
const unsigned int idx, const unsigned int size_x,
const unsigned int target_x, const unsigned int target_y )
{
return std::hypotf(
static_cast<int>( idx % size_x ) - static_cast<int>( target_x ),
static_cast<int>( idx / size_x ) - static_cast<int>( target_y ) );
}
389 void NodeHybrid::resetObstacleHeuristic(
390 nav2_costmap_2d::Costmap2D * costmap,
const unsigned int & start_x, const unsigned int & start_y,
const unsigned int & goal_x, const unsigned int & goal_y )
{
// Downsample costmap 2x to compute a sparse obstacle heuristic. This speeds up
// the planner considerably to search through 75% less cells with no detectable
// erosion of path quality after even modest smoothing. The error would be no more
// than 0.05 * normalized cost. Since this is just a search prior, there's no loss in generality
std::weak_ptr<nav2_util::LifecycleNode> ptr;
downsampler.on_configure( ptr, "fake_frame", "fake_topic", costmap, 2.0, true );
downsampler.on_activate( );
sampled_costmap = downsampler.downsample( 2.0 );
// Clear lookup table
unsigned int size = sampled_costmap->getSizeInCellsX( ) * sampled_costmap->getSizeInCellsY( );
if ( obstacle_heuristic_lookup_table.size( ) == size ) {
// must reset all values
std::fill(
obstacle_heuristic_lookup_table.begin( ),
obstacle_heuristic_lookup_table.end( ), 0.0 );
} else {
unsigned int obstacle_size = obstacle_heuristic_lookup_table.size( );
obstacle_heuristic_lookup_table.resize( size, 0.0 );
// must reset values for non-constructed indices
std::fill_n(
obstacle_heuristic_lookup_table.begin( ), obstacle_size, 0.0 );
}
obstacle_heuristic_queue.clear( );
obstacle_heuristic_queue.reserve(
sampled_costmap->getSizeInCellsX( ) * sampled_costmap->getSizeInCellsY( ) );
// Set initial goal point to queue from. Divided by 2 due to downsampled costmap.
const unsigned int size_x = sampled_costmap->getSizeInCellsX( );
const unsigned int goal_index = floor( goal_y / 2.0 ) * size_x + floor( goal_x / 2.0 );
obstacle_heuristic_queue.emplace_back(
distanceHeuristic2D( goal_index, size_x, start_x, start_y ), goal_index );
// initialize goal cell with a very small value to differentiate it from 0.0 ( ~uninitialized )
// the negative value means the cell is in the open set
obstacle_heuristic_lookup_table[goal_index] = -0.00001f;
}
433 float NodeHybrid::getObstacleHeuristic(
434 const Coordinates & node_coords,
435 const Coordinates & goal_coords,
const double & cost_penalty )
{
// If already expanded, return the cost
const unsigned int size_x = sampled_costmap->getSizeInCellsX( );
// Divided by 2 due to downsampled costmap.
const unsigned int start_y = floor( node_coords.y / 2.0 );
const unsigned int start_x = floor( node_coords.x / 2.0 );
const unsigned int start_index = start_y * size_x + start_x;
const float & requested_node_cost = obstacle_heuristic_lookup_table[start_index];
if ( requested_node_cost > 0.0f ) {
// costs are doubled due to downsampling
return 2.0 * requested_node_cost;
}
// If not, expand until it is included. This dynamic programming ensures that
// we only expand the MINIMUM spanning set of the costmap per planning request.
// Rather than naively expanding the entire ( potentially massive ) map for a limited
// path, we only expand to the extent required for the furthest expansion in the
// search-planning request that dynamically updates during search as needed.
// start_x and start_y have changed since last call
// we need to recompute 2D distance heuristic and reprioritize queue
for ( auto & n : obstacle_heuristic_queue ) {
n.first = -obstacle_heuristic_lookup_table[n.second] +
distanceHeuristic2D( n.second, size_x, start_x, start_y );
}
std::make_heap(
obstacle_heuristic_queue.begin( ), obstacle_heuristic_queue.end( ),
ObstacleHeuristicComparator{} );
const int size_x_int = static_cast<int>( size_x );
const unsigned int size_y = sampled_costmap->getSizeInCellsY( );
const float sqrt_2 = sqrt( 2 );
float c_cost, cost, travel_cost, new_cost, existing_cost;
unsigned int idx, mx, my, mx_idx, my_idx;
unsigned int new_idx = 0;
const std::vector<int> neighborhood = {1, -1, // left right
size_x_int, -size_x_int, // up down
size_x_int + 1, size_x_int - 1, // upper diagonals
-size_x_int + 1, -size_x_int - 1}; // lower diagonals
while ( !obstacle_heuristic_queue.empty( ) ) {
idx = obstacle_heuristic_queue.front( ).second;
std::pop_heap(
obstacle_heuristic_queue.begin( ), obstacle_heuristic_queue.end( ),
ObstacleHeuristicComparator{} );
obstacle_heuristic_queue.pop_back( );
c_cost = obstacle_heuristic_lookup_table[idx];
if ( c_cost > 0.0f ) {
// cell has been processed and closed, no further cost improvements
// are mathematically possible thanks to euclidean distance heuristic consistency
continue;
}
c_cost = -c_cost;
obstacle_heuristic_lookup_table[idx] = c_cost; // set a positive value to close the cell
my_idx = idx / size_x;
mx_idx = idx - ( my_idx * size_x );
// find neighbors
for ( unsigned int i = 0; i != neighborhood.size( ); i++ ) {
new_idx = static_cast<unsigned int>( static_cast<int>( idx ) + neighborhood[i] );
// if neighbor path is better and non-lethal, set new cost and add to queue
if ( new_idx < size_x * size_y ) {
cost = static_cast<float>( sampled_costmap->getCost( new_idx ) );
if ( cost >= INSCRIBED ) {
continue;
}
my = new_idx / size_x;
mx = new_idx - ( my * size_x );
if ( mx == 0 && mx_idx >= size_x - 1 || mx >= size_x - 1 && mx_idx == 0 ) {
continue;
}
if ( my == 0 && my_idx >= size_y - 1 || my >= size_y - 1 && my_idx == 0 ) {
continue;
}
existing_cost = obstacle_heuristic_lookup_table[new_idx];
if ( existing_cost <= 0.0f ) {
travel_cost =
( ( i <= 3 ) ? 1.0f : sqrt_2 ) * ( 1.0f + ( cost_penalty * cost / 252.0f ) );
new_cost = c_cost + travel_cost;
if ( existing_cost == 0.0f || -existing_cost > new_cost ) {
// the negative value means the cell is in the open set
obstacle_heuristic_lookup_table[new_idx] = -new_cost;
obstacle_heuristic_queue.emplace_back(
new_cost + distanceHeuristic2D( new_idx, size_x, start_x, start_y ), new_idx );
std::push_heap(
obstacle_heuristic_queue.begin( ), obstacle_heuristic_queue.end( ),
ObstacleHeuristicComparator{} );
}
}
}
}
if ( idx == start_index ) {
break;
}
}
// return requested_node_cost which has been updated by the search
// costs are doubled due to downsampling
return 2.0 * requested_node_cost;
}
545 float NodeHybrid::getDistanceHeuristic(
546 const Coordinates & node_coords,
547 const Coordinates & goal_coords,
const float & obstacle_heuristic )
{
// rotate and translate node_coords such that goal_coords relative is ( 0, 0, 0 )
// Due to the rounding involved in exact cell increments for caching,
// this is not an exact replica of a live heuristic, but has bounded error.
// ( Usually less than 1 cell )
// This angle is negative since we are de-rotating the current node
// by the goal angle; cos( -th ) = cos( th ) & sin( -th ) = -sin( th )
const TrigValues & trig_vals = motion_table.trig_values[goal_coords.theta];
const float cos_th = trig_vals.first;
const float sin_th = -trig_vals.second;
const float dx = node_coords.x - goal_coords.x;
const float dy = node_coords.y - goal_coords.y;
double dtheta_bin = node_coords.theta - goal_coords.theta;
if ( dtheta_bin < 0 ) {
dtheta_bin += motion_table.num_angle_quantization;
}
if ( dtheta_bin > motion_table.num_angle_quantization ) {
dtheta_bin -= motion_table.num_angle_quantization;
}
Coordinates node_coords_relative(
round( dx * cos_th - dy * sin_th ),
round( dx * sin_th + dy * cos_th ),
round( dtheta_bin ) );
// Check if the relative node coordinate is within the localized window around the goal
// to apply the distance heuristic. Since the lookup table is contains only the positive
// X axis, we mirror the Y and theta values across the X axis to find the heuristic values.
float motion_heuristic = 0.0;
const int floored_size = floor( size_lookup / 2.0 );
const int ceiling_size = ceil( size_lookup / 2.0 );
const float mirrored_relative_y = abs( node_coords_relative.y );
if ( abs( node_coords_relative.x ) < floored_size && mirrored_relative_y < floored_size ) {
// Need to mirror angle if Y coordinate was mirrored
int theta_pos;
if ( node_coords_relative.y < 0.0 ) {
theta_pos = motion_table.num_angle_quantization - node_coords_relative.theta;
} else {
theta_pos = node_coords_relative.theta;
}
const int x_pos = node_coords_relative.x + floored_size;
const int y_pos = static_cast<int>( mirrored_relative_y );
const int index =
x_pos * ceiling_size * motion_table.num_angle_quantization +
y_pos * motion_table.num_angle_quantization +
theta_pos;
motion_heuristic = dist_heuristic_lookup_table[index];
} else if ( obstacle_heuristic == 0.0 ) {
// If no obstacle heuristic value, must have some H to use
// In nominal situations, this should never be called.
static ompl::base::ScopedState<> from( motion_table.state_space ), to( motion_table.state_space );
to[0] = goal_coords.x;
to[1] = goal_coords.y;
to[2] = goal_coords.theta * motion_table.num_angle_quantization;
from[0] = node_coords.x;
from[1] = node_coords.y;
from[2] = node_coords.theta * motion_table.num_angle_quantization;
motion_heuristic = motion_table.state_space->distance( from( ), to( ) );
}
return motion_heuristic;
}
614 void NodeHybrid::precomputeDistanceHeuristic(
const float & lookup_table_dim,
616 const MotionModel & motion_model,
const unsigned int & dim_3_size,
const SearchInfo & search_info )
{
// Dubin or Reeds-Shepp shortest distances
if ( motion_model == MotionModel::DUBIN ) {
motion_table.state_space = std::make_unique<ompl::base::DubinsStateSpace>(
search_info.minimum_turning_radius );
} else if ( motion_model == MotionModel::REEDS_SHEPP ) {
motion_table.state_space = std::make_unique<ompl::base::ReedsSheppStateSpace>(
search_info.minimum_turning_radius );
} else {
throw std::runtime_error(
"Node attempted to precompute distance heuristics "
"with invalid motion model!" );
}
ompl::base::ScopedState<> from( motion_table.state_space ), to( motion_table.state_space );
to[0] = 0.0;
to[1] = 0.0;
to[2] = 0.0;
size_lookup = lookup_table_dim;
float motion_heuristic = 0.0;
unsigned int index = 0;
int dim_3_size_int = static_cast<int>( dim_3_size );
float angular_bin_size = 2 * M_PI / static_cast<float>( dim_3_size );
// Create a lookup table of Dubin/Reeds-Shepp distances in a window around the goal
// to help drive the search towards admissible approaches. Deu to symmetries in the
// Heuristic space, we need to only store 2 of the 4 quadrants and simply mirror
// around the X axis any relative node lookup. This reduces memory overhead and increases
// the size of a window a platform can store in memory.
dist_heuristic_lookup_table.resize( size_lookup * ceil( size_lookup / 2.0 ) * dim_3_size_int );
for ( float x = ceil( -size_lookup / 2.0 ); x <= floor( size_lookup / 2.0 ); x += 1.0 ) {
for ( float y = 0.0; y <= floor( size_lookup / 2.0 ); y += 1.0 ) {
for ( int heading = 0; heading != dim_3_size_int; heading++ ) {
from[0] = x;
from[1] = y;
from[2] = heading * angular_bin_size;
motion_heuristic = motion_table.state_space->distance( from( ), to( ) );
dist_heuristic_lookup_table[index] = motion_heuristic;
index++;
}
}
}
}
663 void NodeHybrid::getNeighbors(
664 std::function<bool( const unsigned int &, nav2_smac_planner::NodeHybrid * & )> & NeighborGetter,
665 GridCollisionChecker * collision_checker,
666 const bool & traverse_unknown,
667 NodeVector & neighbors )
{
unsigned int index = 0;
NodePtr neighbor = nullptr;
Coordinates initial_node_coords;
const MotionPoses motion_projections = motion_table.getProjections( this );
for ( unsigned int i = 0; i != motion_projections.size( ); i++ ) {
index = NodeHybrid::getIndex(
static_cast<unsigned int>( motion_projections[i]._x ),
static_cast<unsigned int>( motion_projections[i]._y ),
static_cast<unsigned int>( motion_projections[i]._theta ),
motion_table.size_x, motion_table.num_angle_quantization );
if ( NeighborGetter( index, neighbor ) && !neighbor->wasVisited( ) ) {
// Cache the initial pose in case it was visited but valid
// don't want to disrupt continuous coordinate expansion
initial_node_coords = neighbor->pose;
neighbor->setPose(
Coordinates(
motion_projections[i]._x,
motion_projections[i]._y,
motion_projections[i]._theta ) );
if ( neighbor->isNodeValid( traverse_unknown, collision_checker ) ) {
neighbor->setMotionPrimitiveIndex( i );
neighbors.push_back( neighbor );
} else {
neighbor->setPose( initial_node_coords );
}
}
}
}
700 bool NodeHybrid::backtracePath( CoordinateVector & path )
{
if ( !this->parent ) {
return false;
}
NodePtr current_node = this;
while ( current_node->parent ) {
path.push_back( current_node->pose );
// Convert angle to radians
path.back( ).theta = NodeHybrid::motion_table.getAngleFromBin( path.back( ).theta );
current_node = current_node->parent;
}
return path.size( ) > 0;
}
} // namespace nav2_smac_planner
// Copyright ( c ) 2021, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <math.h>
#include <chrono>
#include <vector>
#include <memory>
#include <algorithm>
#include <queue>
#include <limits>
#include <string>
#include <fstream>
#include <cmath>
#include "ompl/base/ScopedState.h"
#include "ompl/base/spaces/DubinsStateSpace.h"
#include "ompl/base/spaces/ReedsSheppStateSpace.h"
#include "nav2_smac_planner/node_lattice.hpp"
using namespace std::chrono; // NOLINT
namespace nav2_smac_planner
{
// defining static member for all instance to share
LatticeMotionTable NodeLattice::motion_table;
float NodeLattice::size_lookup = 25;
LookupTable NodeLattice::dist_heuristic_lookup_table;
// Each of these tables are the projected motion models through
// time and space applied to the search on the current node in
// continuous map-coordinates ( e.g. not meters but partial map cells )
// Currently, these are set to project *at minimum* into a neighboring
// cell. Though this could be later modified to project a certain
// amount of time or particular distance forward.
void LatticeMotionTable::initMotionModel(
unsigned int & size_x_in,
SearchInfo & search_info )
{
size_x = size_x_in;
if ( current_lattice_filepath == search_info.lattice_filepath ) {
return;
}
size_x = size_x_in;
change_penalty = search_info.change_penalty;
non_straight_penalty = search_info.non_straight_penalty;
cost_penalty = search_info.cost_penalty;
reverse_penalty = search_info.reverse_penalty;
travel_distance_reward = 1.0f - search_info.retrospective_penalty;
current_lattice_filepath = search_info.lattice_filepath;
allow_reverse_expansion = search_info.allow_reverse_expansion;
rotation_penalty = search_info.rotation_penalty;
// Get the metadata about this minimum control set
lattice_metadata = getLatticeMetadata( current_lattice_filepath );
std::ifstream latticeFile( current_lattice_filepath );
if ( !latticeFile.is_open( ) ) {
throw std::runtime_error( "Could not open lattice file" );
}
nlohmann::json json;
latticeFile >> json;
num_angle_quantization = lattice_metadata.number_of_headings;
if ( !state_space ) {
if ( !allow_reverse_expansion ) {
state_space = std::make_unique<ompl::base::DubinsStateSpace>(
lattice_metadata.min_turning_radius );
} else {
state_space = std::make_unique<ompl::base::ReedsSheppStateSpace>(
lattice_metadata.min_turning_radius );
}
}
// Populate the motion primitives at each heading angle
float prev_start_angle = 0.0;
std::vector<MotionPrimitive> primitives;
nlohmann::json json_primitives = json["primitives"];
for ( unsigned int i = 0; i < json_primitives.size( ); ++i ) {
MotionPrimitive new_primitive;
fromJsonToMotionPrimitive( json_primitives[i], new_primitive );
if ( prev_start_angle != new_primitive.start_angle ) {
motion_primitives.push_back( primitives );
primitives.clear( );
prev_start_angle = new_primitive.start_angle;
}
primitives.push_back( new_primitive );
}
motion_primitives.push_back( primitives );
// Populate useful precomputed values to be leveraged
trig_values.reserve( lattice_metadata.number_of_headings );
for ( unsigned int i = 0; i < lattice_metadata.heading_angles.size( ); ++i ) {
trig_values.emplace_back(
cos( lattice_metadata.heading_angles[i] ),
sin( lattice_metadata.heading_angles[i] ) );
}
}
MotionPrimitivePtrs LatticeMotionTable::getMotionPrimitives( const NodeLattice * node )
{
MotionPrimitives & prims_at_heading = motion_primitives[node->pose.theta];
MotionPrimitivePtrs primitive_projection_list;
for ( unsigned int i = 0; i != prims_at_heading.size( ); i++ ) {
primitive_projection_list.push_back( &prims_at_heading[i] );
}
if ( allow_reverse_expansion ) {
// Find normalized heading bin of the reverse expansion
double reserve_heading = node->pose.theta - ( num_angle_quantization / 2 );
if ( reserve_heading < 0 ) {
reserve_heading += num_angle_quantization;
}
if ( reserve_heading > num_angle_quantization ) {
reserve_heading -= num_angle_quantization;
}
MotionPrimitives & prims_at_reverse_heading = motion_primitives[reserve_heading];
for ( unsigned int i = 0; i != prims_at_reverse_heading.size( ); i++ ) {
primitive_projection_list.push_back( &prims_at_reverse_heading[i] );
}
}
return primitive_projection_list;
}
LatticeMetadata LatticeMotionTable::getLatticeMetadata( const std::string & lattice_filepath )
{
std::ifstream lattice_file( lattice_filepath );
if ( !lattice_file.is_open( ) ) {
throw std::runtime_error( "Could not open lattice file!" );
}
nlohmann::json j;
lattice_file >> j;
LatticeMetadata metadata;
fromJsonToMetaData( j["lattice_metadata"], metadata );
return metadata;
}
unsigned int LatticeMotionTable::getClosestAngularBin( const double & theta )
{
float min_dist = std::numeric_limits<float>::max( );
unsigned int closest_idx = 0;
float dist = 0.0;
for ( unsigned int i = 0; i != lattice_metadata.heading_angles.size( ); i++ ) {
dist = fabs( angles::shortest_angular_distance( theta, lattice_metadata.heading_angles[i] ) );
if ( dist < min_dist ) {
min_dist = dist;
closest_idx = i;
}
}
return closest_idx;
}
float & LatticeMotionTable::getAngleFromBin( const unsigned int & bin_idx )
{
return lattice_metadata.heading_angles[bin_idx];
}
175 NodeLattice::NodeLattice( const unsigned int index )
: parent( nullptr ),
177 pose( 0.0f, 0.0f, 0.0f ),
_cell_cost( std::numeric_limits<float>::quiet_NaN( ) ),
_accumulated_cost( std::numeric_limits<float>::max( ) ),
_index( index ),
_was_visited( false ),
_motion_primitive( nullptr ),
_backwards( false )
{
}
187 NodeLattice::~NodeLattice( )
{
parent = nullptr;
}
192 void NodeLattice::reset( )
{
parent = nullptr;
_cell_cost = std::numeric_limits<float>::quiet_NaN( );
_accumulated_cost = std::numeric_limits<float>::max( );
_was_visited = false;
pose.x = 0.0f;
pose.y = 0.0f;
pose.theta = 0.0f;
_motion_primitive = nullptr;
_backwards = false;
}
205 bool NodeLattice::isNodeValid(
206 const bool & traverse_unknown,
207 GridCollisionChecker * collision_checker,
MotionPrimitive * motion_primitive,
209 bool is_backwards )
{
// Check primitive end pose
// Convert grid quantization of primitives to radians, then collision checker quantization
static const double bin_size = 2.0 * M_PI / collision_checker->getPrecomputedAngles( ).size( );
const double & angle = motion_table.getAngleFromBin( this->pose.theta ) / bin_size;
if ( collision_checker->inCollision(
this->pose.x, this->pose.y, angle /*bin in collision checker*/, traverse_unknown ) )
{
return false;
}
// Set the cost of a node to the highest cost across the primitive
float max_cell_cost = collision_checker->getCost( );
// If valid motion primitives are set, check intermediary poses > 1 cell apart
if ( motion_primitive ) {
const float pi_2 = 2.0 * M_PI;
const float & grid_resolution = motion_table.lattice_metadata.grid_resolution;
const float & resolution_diag_sq = 2.0 * grid_resolution * grid_resolution;
MotionPose last_pose( 1e9, 1e9, 1e9 ), pose_dist( 0.0, 0.0, 0.0 );
// Back out the initial node starting point to move motion primitive relative to
MotionPose initial_pose, prim_pose;
initial_pose._x = this->pose.x - ( motion_primitive->poses.back( )._x / grid_resolution );
initial_pose._y = this->pose.y - ( motion_primitive->poses.back( )._y / grid_resolution );
initial_pose._theta = motion_table.getAngleFromBin( motion_primitive->start_angle );
for ( auto it = motion_primitive->poses.begin( ); it != motion_primitive->poses.end( ); ++it ) {
// poses are in metric coordinates from ( 0, 0 ), not grid space yet
pose_dist = *it - last_pose;
// Avoid square roots by ( hypot( x, y ) > res ) == ( x*x+y*y > diag*diag )
if ( pose_dist._x * pose_dist._x + pose_dist._y * pose_dist._y > resolution_diag_sq ) {
last_pose = *it;
// Convert primitive pose into grid space if it should be checked
prim_pose._x = initial_pose._x + ( it->_x / grid_resolution );
prim_pose._y = initial_pose._y + ( it->_y / grid_resolution );
// If reversing, invert the angle because the robot is backing into the primitive
// not driving forward with it
if ( is_backwards ) {
prim_pose._theta = std::fmod( it->_theta + M_PI, pi_2 );
} else {
prim_pose._theta = it->_theta;
}
if ( collision_checker->inCollision(
prim_pose._x,
prim_pose._y,
prim_pose._theta / bin_size /*bin in collision checker*/,
traverse_unknown ) )
{
return false;
}
max_cell_cost = std::max( max_cell_cost, collision_checker->getCost( ) );
}
}
}
_cell_cost = max_cell_cost;
return true;
}
270 float NodeLattice::getTraversalCost( const NodePtr & child )
{
const float normalized_cost = child->getCost( ) / 252.0;
if ( std::isnan( normalized_cost ) ) {
throw std::runtime_error(
"Node attempted to get traversal "
"cost without a known collision cost!" );
}
// this is the first node
MotionPrimitive * prim = this->getMotionPrimitive( );
MotionPrimitive * transition_prim = child->getMotionPrimitive( );
const float prim_length =
transition_prim->trajectory_length / motion_table.lattice_metadata.grid_resolution;
if ( prim == nullptr ) {
return prim_length;
}
// Pure rotation in place 1 angular bin in either direction
if ( transition_prim->trajectory_length < 1e-4 ) {
return motion_table.rotation_penalty * ( 1.0 + motion_table.cost_penalty * normalized_cost );
}
float travel_cost = 0.0;
float travel_cost_raw = prim_length *
( motion_table.travel_distance_reward + motion_table.cost_penalty * normalized_cost );
if ( transition_prim->arc_length < 0.001 ) {
// New motion is a straight motion, no additional costs to be applied
travel_cost = travel_cost_raw;
} else {
if ( prim->left_turn == transition_prim->left_turn ) {
// Turning motion but keeps in same general direction: encourages to commit to actions
travel_cost = travel_cost_raw * motion_table.non_straight_penalty;
} else {
// Turning motion and velocity directions: penalizes wiggling.
travel_cost = travel_cost_raw *
( motion_table.non_straight_penalty + motion_table.change_penalty );
}
}
// If backwards flag is set, this primitive is moving in reverse
if ( child->isBackward( ) ) {
// reverse direction
travel_cost *= motion_table.reverse_penalty;
}
return travel_cost;
}
320 float NodeLattice::getHeuristicCost(
321 const Coordinates & node_coords,
322 const Coordinates & goal_coords,
323 const nav2_costmap_2d::Costmap2D * costmap )
{
// get obstacle heuristic value
const float obstacle_heuristic = getObstacleHeuristic(
node_coords, goal_coords, motion_table.cost_penalty );
const float distance_heuristic =
getDistanceHeuristic( node_coords, goal_coords, obstacle_heuristic );
return std::max( obstacle_heuristic, distance_heuristic );
}
333 void NodeLattice::initMotionModel(
334 const MotionModel & motion_model,
unsigned int & size_x,
unsigned int & /*size_y*/,
unsigned int & /*num_angle_quantization*/,
SearchInfo & search_info )
{
if ( motion_model != MotionModel::STATE_LATTICE ) {
throw std::runtime_error(
"Invalid motion model for Lattice node. Please select"
" STATE_LATTICE and provide a valid lattice file." );
}
motion_table.initMotionModel( size_x, search_info );
}
349 float NodeLattice::getDistanceHeuristic(
350 const Coordinates & node_coords,
351 const Coordinates & goal_coords,
const float & obstacle_heuristic )
{
// rotate and translate node_coords such that goal_coords relative is ( 0, 0, 0 )
// Due to the rounding involved in exact cell increments for caching,
// this is not an exact replica of a live heuristic, but has bounded error.
// ( Usually less than 1 cell length )
// This angle is negative since we are de-rotating the current node
// by the goal angle; cos( -th ) = cos( th ) & sin( -th ) = -sin( th )
const TrigValues & trig_vals = motion_table.trig_values[goal_coords.theta];
const float cos_th = trig_vals.first;
const float sin_th = -trig_vals.second;
const float dx = node_coords.x - goal_coords.x;
const float dy = node_coords.y - goal_coords.y;
double dtheta_bin = node_coords.theta - goal_coords.theta;
if ( dtheta_bin < 0 ) {
dtheta_bin += motion_table.num_angle_quantization;
}
if ( dtheta_bin > motion_table.num_angle_quantization ) {
dtheta_bin -= motion_table.num_angle_quantization;
}
Coordinates node_coords_relative(
round( dx * cos_th - dy * sin_th ),
round( dx * sin_th + dy * cos_th ),
round( dtheta_bin ) );
// Check if the relative node coordinate is within the localized window around the goal
// to apply the distance heuristic. Since the lookup table is contains only the positive
// X axis, we mirror the Y and theta values across the X axis to find the heuristic values.
float motion_heuristic = 0.0;
const int floored_size = floor( size_lookup / 2.0 );
const int ceiling_size = ceil( size_lookup / 2.0 );
const float mirrored_relative_y = abs( node_coords_relative.y );
if ( abs( node_coords_relative.x ) < floored_size && mirrored_relative_y < floored_size ) {
// Need to mirror angle if Y coordinate was mirrored
int theta_pos;
if ( node_coords_relative.y < 0.0 ) {
theta_pos = motion_table.num_angle_quantization - node_coords_relative.theta;
} else {
theta_pos = node_coords_relative.theta;
}
const int x_pos = node_coords_relative.x + floored_size;
const int y_pos = static_cast<int>( mirrored_relative_y );
const int index =
x_pos * ceiling_size * motion_table.num_angle_quantization +
y_pos * motion_table.num_angle_quantization +
theta_pos;
motion_heuristic = dist_heuristic_lookup_table[index];
} else if ( obstacle_heuristic == 0.0 ) {
static ompl::base::ScopedState<> from( motion_table.state_space ), to( motion_table.state_space );
to[0] = goal_coords.x;
to[1] = goal_coords.y;
to[2] = motion_table.getAngleFromBin( goal_coords.theta );
from[0] = node_coords.x;
from[1] = node_coords.y;
from[2] = motion_table.getAngleFromBin( node_coords.theta );
motion_heuristic = motion_table.state_space->distance( from( ), to( ) );
}
return motion_heuristic;
}
416 void NodeLattice::precomputeDistanceHeuristic(
const float & lookup_table_dim,
418 const MotionModel & motion_model,
const unsigned int & dim_3_size,
const SearchInfo & search_info )
{
// Dubin or Reeds-Shepp shortest distances
if ( !search_info.allow_reverse_expansion ) {
motion_table.state_space = std::make_unique<ompl::base::DubinsStateSpace>(
search_info.minimum_turning_radius );
} else {
motion_table.state_space = std::make_unique<ompl::base::ReedsSheppStateSpace>(
search_info.minimum_turning_radius );
}
motion_table.lattice_metadata =
LatticeMotionTable::getLatticeMetadata( search_info.lattice_filepath );
ompl::base::ScopedState<> from( motion_table.state_space ), to( motion_table.state_space );
to[0] = 0.0;
to[1] = 0.0;
to[2] = 0.0;
size_lookup = lookup_table_dim;
float motion_heuristic = 0.0;
unsigned int index = 0;
int dim_3_size_int = static_cast<int>( dim_3_size );
// Create a lookup table of Dubin/Reeds-Shepp distances in a window around the goal
// to help drive the search towards admissible approaches. Deu to symmetries in the
// Heuristic space, we need to only store 2 of the 4 quadrants and simply mirror
// around the X axis any relative node lookup. This reduces memory overhead and increases
// the size of a window a platform can store in memory.
dist_heuristic_lookup_table.resize( size_lookup * ceil( size_lookup / 2.0 ) * dim_3_size_int );
for ( float x = ceil( -size_lookup / 2.0 ); x <= floor( size_lookup / 2.0 ); x += 1.0 ) {
for ( float y = 0.0; y <= floor( size_lookup / 2.0 ); y += 1.0 ) {
for ( int heading = 0; heading != dim_3_size_int; heading++ ) {
from[0] = x;
from[1] = y;
from[2] = motion_table.getAngleFromBin( heading );
motion_heuristic = motion_table.state_space->distance( from( ), to( ) );
dist_heuristic_lookup_table[index] = motion_heuristic;
index++;
}
}
}
}
462 void NodeLattice::getNeighbors(
463 std::function<bool( const unsigned int &, nav2_smac_planner::NodeLattice * & )> & NeighborGetter,
464 GridCollisionChecker * collision_checker,
465 const bool & traverse_unknown,
466 NodeVector & neighbors )
{
unsigned int index = 0;
float angle;
bool backwards = false;
NodePtr neighbor = nullptr;
Coordinates initial_node_coords, motion_projection;
MotionPrimitivePtrs motion_primitives = motion_table.getMotionPrimitives( this );
const float & grid_resolution = motion_table.lattice_metadata.grid_resolution;
unsigned int direction_change_idx = 1e9;
for ( unsigned int i = 0; i != motion_primitives.size( ); i++ ) {
if ( motion_primitives[0]->start_angle != motion_primitives[i]->start_angle ) {
direction_change_idx = i;
break;
}
}
for ( unsigned int i = 0; i != motion_primitives.size( ); i++ ) {
const MotionPose & end_pose = motion_primitives[i]->poses.back( );
motion_projection.x = this->pose.x + ( end_pose._x / grid_resolution );
motion_projection.y = this->pose.y + ( end_pose._y / grid_resolution );
motion_projection.theta = motion_primitives[i]->end_angle /*this is the ending angular bin*/;
index = NodeLattice::getIndex(
static_cast<unsigned int>( motion_projection.x ),
static_cast<unsigned int>( motion_projection.y ),
static_cast<unsigned int>( motion_projection.theta ) );
if ( NeighborGetter( index, neighbor ) && !neighbor->wasVisited( ) ) {
// Cache the initial pose in case it was visited but valid
// don't want to disrupt continuous coordinate expansion
initial_node_coords = neighbor->pose;
// if i >= idx, then we're in a reversing primitive. In that situation,
// the orientation of the robot is mirrored from what it would otherwise
// appear to be from the motion primitives file. We want to take this into
// account in case the robot base footprint is asymmetric.
backwards = false;
angle = motion_projection.theta;
if ( i >= direction_change_idx ) {
backwards = true;
angle = motion_projection.theta - ( motion_table.num_angle_quantization / 2 );
if ( angle < 0 ) {
angle += motion_table.num_angle_quantization;
}
if ( angle > motion_table.num_angle_quantization ) {
angle -= motion_table.num_angle_quantization;
}
}
neighbor->setPose(
Coordinates(
motion_projection.x,
motion_projection.y,
angle ) );
// Using a special isNodeValid API here, giving the motion primitive to use to
// validity check the transition of the current node to the new node over
if ( neighbor->isNodeValid(
traverse_unknown, collision_checker, motion_primitives[i], backwards ) )
{
neighbor->setMotionPrimitive( motion_primitives[i] );
// Marking if this search was obtained in the reverse direction
neighbor->backwards( backwards );
neighbors.push_back( neighbor );
} else {
neighbor->setPose( initial_node_coords );
}
}
}
}
538 bool NodeLattice::backtracePath( CoordinateVector & path )
{
if ( !this->parent ) {
return false;
}
Coordinates initial_pose, prim_pose;
NodePtr current_node = this;
MotionPrimitive * prim = nullptr;
const float & grid_resolution = NodeLattice::motion_table.lattice_metadata.grid_resolution;
const float pi_2 = 2.0 * M_PI;
while ( current_node->parent ) {
prim = current_node->getMotionPrimitive( );
// if motion primitive is valid, then was searched ( rather than analytically expanded ),
// include dense path of subpoints making up the primitive at grid resolution
if ( prim ) {
initial_pose.x = current_node->pose.x - ( prim->poses.back( )._x / grid_resolution );
initial_pose.y = current_node->pose.y - ( prim->poses.back( )._y / grid_resolution );
initial_pose.theta = NodeLattice::motion_table.getAngleFromBin( prim->start_angle );
for ( auto it = prim->poses.crbegin( ); it != prim->poses.crend( ); ++it ) {
// Convert primitive pose into grid space if it should be checked
prim_pose.x = initial_pose.x + ( it->_x / grid_resolution );
prim_pose.y = initial_pose.y + ( it->_y / grid_resolution );
// If reversing, invert the angle because the robot is backing into the primitive
// not driving forward with it
if ( current_node->isBackward( ) ) {
prim_pose.theta = std::fmod( it->_theta + M_PI, pi_2 );
} else {
prim_pose.theta = it->_theta;
}
path.push_back( prim_pose );
}
} else {
// For analytic expansion nodes where there is no valid motion primitive
path.push_back( current_node->pose );
path.back( ).theta = NodeLattice::motion_table.getAngleFromBin( path.back( ).theta );
}
current_node = current_node->parent;
}
return path.size( ) > 0;
}
} // namespace nav2_smac_planner
1 // Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <string>
#include <memory>
#include <vector>
#include <limits>
#include <algorithm>
#include "nav2_smac_planner/smac_planner_2d.hpp"
#include "nav2_util/geometry_utils.hpp"
// #define BENCHMARK_TESTING
namespace nav2_smac_planner
{
using namespace std::chrono; // NOLINT
using rcl_interfaces::msg::ParameterType;
using std::placeholders::_1;
32 SmacPlanner2D::SmacPlanner2D( )
: _a_star( nullptr ),
_collision_checker( nullptr, 1 ),
_smoother( nullptr ),
_costmap( nullptr ),
_costmap_downsampler( nullptr )
{
}
41 SmacPlanner2D::~SmacPlanner2D( )
{
RCLCPP_INFO(
_logger, "Destroying plugin %s of type SmacPlanner2D",
_name.c_str( ) );
}
48 void SmacPlanner2D::configure(
49 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
50 std::string name, std::shared_ptr<tf2_ros::Buffer>/*tf*/,
51 std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros )
{
_node = parent;
auto node = parent.lock( );
_logger = node->get_logger( );
_clock = node->get_clock( );
_costmap = costmap_ros->getCostmap( );
_name = name;
_global_frame = costmap_ros->getGlobalFrameID( );
RCLCPP_INFO( _logger, "Configuring %s of type SmacPlanner2D", name.c_str( ) );
// General planner params
nav2_util::declare_parameter_if_not_declared(
node, name + ".tolerance", rclcpp::ParameterValue( 0.125 ) );
_tolerance = static_cast<float>( node->get_parameter( name + ".tolerance" ).as_double( ) );
nav2_util::declare_parameter_if_not_declared(
node, name + ".downsample_costmap", rclcpp::ParameterValue( false ) );
node->get_parameter( name + ".downsample_costmap", _downsample_costmap );
nav2_util::declare_parameter_if_not_declared(
node, name + ".downsampling_factor", rclcpp::ParameterValue( 1 ) );
node->get_parameter( name + ".downsampling_factor", _downsampling_factor );
nav2_util::declare_parameter_if_not_declared(
node, name + ".cost_travel_multiplier", rclcpp::ParameterValue( 1.0 ) );
node->get_parameter( name + ".cost_travel_multiplier", _search_info.cost_penalty );
nav2_util::declare_parameter_if_not_declared(
node, name + ".allow_unknown", rclcpp::ParameterValue( true ) );
node->get_parameter( name + ".allow_unknown", _allow_unknown );
nav2_util::declare_parameter_if_not_declared(
node, name + ".max_iterations", rclcpp::ParameterValue( 1000000 ) );
node->get_parameter( name + ".max_iterations", _max_iterations );
nav2_util::declare_parameter_if_not_declared(
node, name + ".max_on_approach_iterations", rclcpp::ParameterValue( 1000 ) );
node->get_parameter( name + ".max_on_approach_iterations", _max_on_approach_iterations );
nav2_util::declare_parameter_if_not_declared(
node, name + ".use_final_approach_orientation", rclcpp::ParameterValue( false ) );
node->get_parameter( name + ".use_final_approach_orientation", _use_final_approach_orientation );
nav2_util::declare_parameter_if_not_declared(
node, name + ".max_planning_time", rclcpp::ParameterValue( 2.0 ) );
node->get_parameter( name + ".max_planning_time", _max_planning_time );
nav2_util::declare_parameter_if_not_declared(
node, name + ".motion_model_for_search", rclcpp::ParameterValue( std::string( "MOORE" ) ) );
node->get_parameter( name + ".motion_model_for_search", _motion_model_for_search );
_motion_model = fromString( _motion_model_for_search );
if ( _motion_model == MotionModel::UNKNOWN ) {
RCLCPP_WARN(
_logger,
"Unable to get MotionModel search type. Given '%s', "
"valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.",
_motion_model_for_search.c_str( ) );
}
if ( _max_on_approach_iterations <= 0 ) {
RCLCPP_INFO(
_logger, "On approach iteration selected as <= 0, "
"disabling tolerance and on approach iterations." );
_max_on_approach_iterations = std::numeric_limits<int>::max( );
}
if ( _max_iterations <= 0 ) {
RCLCPP_INFO(
_logger, "maximum iteration selected as <= 0, "
"disabling maximum iterations." );
_max_iterations = std::numeric_limits<int>::max( );
}
// Initialize collision checker
_collision_checker = GridCollisionChecker( _costmap, 1 /*for 2D, most be 1*/ );
_collision_checker.setFootprint(
costmap_ros->getRobotFootprint( ),
true /*for 2D, most use radius*/,
0.0 /*for 2D cost at inscribed isn't relevent*/ );
// Initialize A* template
_a_star = std::make_unique<AStarAlgorithm<Node2D>>( _motion_model, _search_info );
_a_star->initialize(
_allow_unknown,
_max_iterations,
_max_on_approach_iterations,
_max_planning_time,
0.0 /*unused for 2D*/,
1.0 /*unused for 2D*/ );
// Initialize path smoother
SmootherParams params;
params.get( node, name );
params.holonomic_ = true; // So smoother will treat this as a grid search
_smoother = std::make_unique<Smoother>( params );
_smoother->initialize( 1e-50 /*No valid minimum turning radius for 2D*/ );
// Initialize costmap downsampler
if ( _downsample_costmap && _downsampling_factor > 1 ) {
std::string topic_name = "downsampled_costmap";
_costmap_downsampler = std::make_unique<CostmapDownsampler>( );
_costmap_downsampler->on_configure(
node, _global_frame, topic_name, _costmap, _downsampling_factor );
}
_raw_plan_publisher = node->create_publisher<nav_msgs::msg::Path>( "unsmoothed_plan", 1 );
RCLCPP_INFO(
_logger, "Configured plugin %s of type SmacPlanner2D with "
"tolerance %.2f, maximum iterations %i, "
"max on approach iterations %i, and %s. Using motion model: %s.",
_name.c_str( ), _tolerance, _max_iterations, _max_on_approach_iterations,
_allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal",
toString( _motion_model ).c_str( ) );
}
163 void SmacPlanner2D::activate( )
{
RCLCPP_INFO(
_logger, "Activating plugin %s of type SmacPlanner2D",
_name.c_str( ) );
_raw_plan_publisher->on_activate( );
if ( _costmap_downsampler ) {
_costmap_downsampler->on_activate( );
}
auto node = _node.lock( );
// Add callback for dynamic parameters
_dyn_params_handler = node->add_on_set_parameters_callback(
std::bind( &SmacPlanner2D::dynamicParametersCallback, this, _1 ) );
}
178 void SmacPlanner2D::deactivate( )
{
RCLCPP_INFO(
_logger, "Deactivating plugin %s of type SmacPlanner2D",
_name.c_str( ) );
_raw_plan_publisher->on_deactivate( );
if ( _costmap_downsampler ) {
_costmap_downsampler->on_deactivate( );
}
_dyn_params_handler.reset( );
}
190 void SmacPlanner2D::cleanup( )
{
RCLCPP_INFO(
_logger, "Cleaning up plugin %s of type SmacPlanner2D",
_name.c_str( ) );
_a_star.reset( );
_smoother.reset( );
if ( _costmap_downsampler ) {
_costmap_downsampler->on_cleanup( );
_costmap_downsampler.reset( );
}
_raw_plan_publisher.reset( );
}
204 nav_msgs::msg::Path SmacPlanner2D::createPlan(
205 const geometry_msgs::msg::PoseStamped & start,
206 const geometry_msgs::msg::PoseStamped & goal )
{
std::lock_guard<std::mutex> lock_reinit( _mutex );
steady_clock::time_point a = steady_clock::now( );
std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock( *( _costmap->getMutex( ) ) );
// Downsample costmap, if required
nav2_costmap_2d::Costmap2D * costmap = _costmap;
if ( _costmap_downsampler ) {
costmap = _costmap_downsampler->downsample( _downsampling_factor );
_collision_checker.setCostmap( costmap );
}
// Set collision checker and costmap information
_a_star->setCollisionChecker( &_collision_checker );
// Set starting point
unsigned int mx_start, my_start, mx_goal, my_goal;
costmap->worldToMap( start.pose.position.x, start.pose.position.y, mx_start, my_start );
_a_star->setStart( mx_start, my_start, 0 );
// Set goal point
costmap->worldToMap( goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal );
_a_star->setGoal( mx_goal, my_goal, 0 );
// Setup message
nav_msgs::msg::Path plan;
plan.header.stamp = _clock->now( );
plan.header.frame_id = _global_frame;
geometry_msgs::msg::PoseStamped pose;
pose.header = plan.header;
pose.pose.position.z = 0.0;
pose.pose.orientation.x = 0.0;
pose.pose.orientation.y = 0.0;
pose.pose.orientation.z = 0.0;
pose.pose.orientation.w = 1.0;
// Corner case of start and goal beeing on the same cell
if ( mx_start == mx_goal && my_start == my_goal ) {
if ( costmap->getCost( mx_start, my_start ) == nav2_costmap_2d::LETHAL_OBSTACLE ) {
RCLCPP_WARN( _logger, "Failed to create a unique pose path because of obstacles" );
return plan;
}
pose.pose = start.pose;
// if we have a different start and goal orientation, set the unique path pose to the goal
// orientation, unless use_final_approach_orientation=true where we need it to be the start
// orientation to avoid movement from the local planner
if ( start.pose.orientation != goal.pose.orientation && !_use_final_approach_orientation ) {
pose.pose.orientation = goal.pose.orientation;
}
plan.poses.push_back( pose );
return plan;
}
// Compute plan
Node2D::CoordinateVector path;
int num_iterations = 0;
std::string error;
try {
if ( !_a_star->createPath(
path, num_iterations, _tolerance / static_cast<float>( costmap->getResolution( ) ) ) )
{
if ( num_iterations < _a_star->getMaxIterations( ) ) {
error = std::string( "no valid path found" );
} else {
error = std::string( "exceeded maximum iterations" );
}
}
} catch ( const std::runtime_error & e ) {
error = "invalid use: ";
error += e.what( );
}
if ( !error.empty( ) ) {
RCLCPP_WARN(
_logger,
"%s: failed to create plan, %s.",
_name.c_str( ), error.c_str( ) );
return plan;
}
// Convert to world coordinates
plan.poses.reserve( path.size( ) );
for ( int i = path.size( ) - 1; i >= 0; --i ) {
pose.pose = getWorldCoords( path[i].x, path[i].y, costmap );
plan.poses.push_back( pose );
}
// Publish raw path for debug
if ( _raw_plan_publisher->get_subscription_count( ) > 0 ) {
_raw_plan_publisher->publish( plan );
}
// Find how much time we have left to do smoothing
steady_clock::time_point b = steady_clock::now( );
duration<double> time_span = duration_cast<duration<double>>( b - a );
double time_remaining = _max_planning_time - static_cast<double>( time_span.count( ) );
#ifdef BENCHMARK_TESTING
std::cout << "It took " << time_span.count( ) * 1000 <<
" milliseconds with " << num_iterations << " iterations." << std::endl;
#endif
// Smooth plan
_smoother->smooth( plan, costmap, time_remaining );
// If use_final_approach_orientation=true, interpolate the last pose orientation from the
// previous pose to set the orientation to the 'final approach' orientation of the robot so
// it does not rotate.
// And deal with corner case of plan of length 1
// If use_final_approach_orientation=false ( default ), override last pose orientation to match goal
size_t plan_size = plan.poses.size( );
if ( _use_final_approach_orientation ) {
if ( plan_size == 1 ) {
plan.poses.back( ).pose.orientation = start.pose.orientation;
} else if ( plan_size > 1 ) {
double dx, dy, theta;
auto last_pose = plan.poses.back( ).pose.position;
auto approach_pose = plan.poses[plan_size - 2].pose.position;
dx = last_pose.x - approach_pose.x;
dy = last_pose.y - approach_pose.y;
theta = atan2( dy, dx );
plan.poses.back( ).pose.orientation =
nav2_util::geometry_utils::orientationAroundZAxis( theta );
}
} else if ( plan_size > 0 ) {
plan.poses.back( ).pose.orientation = goal.pose.orientation;
}
return plan;
}
rcl_interfaces::msg::SetParametersResult
340 SmacPlanner2D::dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters )
{
rcl_interfaces::msg::SetParametersResult result;
std::lock_guard<std::mutex> lock_reinit( _mutex );
bool reinit_a_star = false;
bool reinit_downsampler = false;
for ( auto parameter : parameters ) {
const auto & type = parameter.get_type( );
const auto & name = parameter.get_name( );
if ( type == ParameterType::PARAMETER_DOUBLE ) {
if ( name == _name + ".tolerance" ) {
_tolerance = static_cast<float>( parameter.as_double( ) );
} else if ( name == _name + ".cost_travel_multiplier" ) {
reinit_a_star = true;
_search_info.cost_penalty = parameter.as_double( );
} else if ( name == _name + ".max_planning_time" ) {
reinit_a_star = true;
_max_planning_time = parameter.as_double( );
}
} else if ( type == ParameterType::PARAMETER_BOOL ) {
if ( name == _name + ".downsample_costmap" ) {
reinit_downsampler = true;
_downsample_costmap = parameter.as_bool( );
} else if ( name == _name + ".allow_unknown" ) {
reinit_a_star = true;
_allow_unknown = parameter.as_bool( );
} else if ( name == _name + ".use_final_approach_orientation" ) {
_use_final_approach_orientation = parameter.as_bool( );
}
} else if ( type == ParameterType::PARAMETER_INTEGER ) {
if ( name == _name + ".downsampling_factor" ) {
reinit_downsampler = true;
_downsampling_factor = parameter.as_int( );
} else if ( name == _name + ".max_iterations" ) {
reinit_a_star = true;
_max_iterations = parameter.as_int( );
if ( _max_iterations <= 0 ) {
RCLCPP_INFO(
_logger, "maximum iteration selected as <= 0, "
"disabling maximum iterations." );
_max_iterations = std::numeric_limits<int>::max( );
}
} else if ( name == _name + ".max_on_approach_iterations" ) {
reinit_a_star = true;
_max_on_approach_iterations = parameter.as_int( );
if ( _max_on_approach_iterations <= 0 ) {
RCLCPP_INFO(
_logger, "On approach iteration selected as <= 0, "
"disabling tolerance and on approach iterations." );
_max_on_approach_iterations = std::numeric_limits<int>::max( );
}
}
} else if ( type == ParameterType::PARAMETER_STRING ) {
if ( name == _name + ".motion_model_for_search" ) {
reinit_a_star = true;
_motion_model = fromString( parameter.as_string( ) );
if ( _motion_model == MotionModel::UNKNOWN ) {
RCLCPP_WARN(
_logger,
"Unable to get MotionModel search type. Given '%s', "
"valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.",
_motion_model_for_search.c_str( ) );
}
}
}
}
// Re-init if needed with mutex lock ( to avoid re-init while creating a plan )
if ( reinit_a_star || reinit_downsampler ) {
// Re-Initialize A* template
if ( reinit_a_star ) {
_a_star = std::make_unique<AStarAlgorithm<Node2D>>( _motion_model, _search_info );
_a_star->initialize(
_allow_unknown,
_max_iterations,
_max_on_approach_iterations,
_max_planning_time,
0.0 /*unused for 2D*/,
1.0 /*unused for 2D*/ );
}
// Re-Initialize costmap downsampler
if ( reinit_downsampler ) {
if ( _downsample_costmap && _downsampling_factor > 1 ) {
auto node = _node.lock( );
std::string topic_name = "downsampled_costmap";
_costmap_downsampler = std::make_unique<CostmapDownsampler>( );
_costmap_downsampler->on_configure(
node, _global_frame, topic_name, _costmap, _downsampling_factor );
}
}
}
result.successful = true;
return result;
}
} // namespace nav2_smac_planner
#include "pluginlib/class_list_macros.hpp"
442 PLUGINLIB_EXPORT_CLASS( nav2_smac_planner::SmacPlanner2D, nav2_core::GlobalPlanner )
1 // Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <string>
#include <memory>
#include <vector>
#include <algorithm>
#include <limits>
#include "Eigen/Core"
#include "nav2_smac_planner/smac_planner_hybrid.hpp"
// #define BENCHMARK_TESTING
namespace nav2_smac_planner
{
using namespace std::chrono; // NOLINT
using rcl_interfaces::msg::ParameterType;
using std::placeholders::_1;
33 SmacPlannerHybrid::SmacPlannerHybrid( )
: _a_star( nullptr ),
_collision_checker( nullptr, 1 ),
_smoother( nullptr ),
_costmap( nullptr ),
_costmap_downsampler( nullptr )
{
}
42 SmacPlannerHybrid::~SmacPlannerHybrid( )
{
RCLCPP_INFO(
_logger, "Destroying plugin %s of type SmacPlannerHybrid",
_name.c_str( ) );
}
49 void SmacPlannerHybrid::configure(
50 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
51 std::string name, std::shared_ptr<tf2_ros::Buffer>/*tf*/,
52 std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros )
{
_node = parent;
auto node = parent.lock( );
_logger = node->get_logger( );
_clock = node->get_clock( );
_costmap = costmap_ros->getCostmap( );
_costmap_ros = costmap_ros;
_name = name;
_global_frame = costmap_ros->getGlobalFrameID( );
RCLCPP_INFO( _logger, "Configuring %s of type SmacPlannerHybrid", name.c_str( ) );
int angle_quantizations;
double analytic_expansion_max_length_m;
bool smooth_path;
// General planner params
nav2_util::declare_parameter_if_not_declared(
node, name + ".downsample_costmap", rclcpp::ParameterValue( false ) );
node->get_parameter( name + ".downsample_costmap", _downsample_costmap );
nav2_util::declare_parameter_if_not_declared(
node, name + ".downsampling_factor", rclcpp::ParameterValue( 1 ) );
node->get_parameter( name + ".downsampling_factor", _downsampling_factor );
nav2_util::declare_parameter_if_not_declared(
node, name + ".angle_quantization_bins", rclcpp::ParameterValue( 72 ) );
node->get_parameter( name + ".angle_quantization_bins", angle_quantizations );
_angle_bin_size = 2.0 * M_PI / angle_quantizations;
_angle_quantizations = static_cast<unsigned int>( angle_quantizations );
nav2_util::declare_parameter_if_not_declared(
node, name + ".allow_unknown", rclcpp::ParameterValue( true ) );
node->get_parameter( name + ".allow_unknown", _allow_unknown );
nav2_util::declare_parameter_if_not_declared(
node, name + ".max_iterations", rclcpp::ParameterValue( 1000000 ) );
node->get_parameter( name + ".max_iterations", _max_iterations );
nav2_util::declare_parameter_if_not_declared(
node, name + ".smooth_path", rclcpp::ParameterValue( true ) );
node->get_parameter( name + ".smooth_path", smooth_path );
nav2_util::declare_parameter_if_not_declared(
node, name + ".minimum_turning_radius", rclcpp::ParameterValue( 0.4 ) );
node->get_parameter( name + ".minimum_turning_radius", _minimum_turning_radius_global_coords );
nav2_util::declare_parameter_if_not_declared(
node, name + ".cache_obstacle_heuristic", rclcpp::ParameterValue( false ) );
node->get_parameter( name + ".cache_obstacle_heuristic", _search_info.cache_obstacle_heuristic );
nav2_util::declare_parameter_if_not_declared(
node, name + ".reverse_penalty", rclcpp::ParameterValue( 2.0 ) );
node->get_parameter( name + ".reverse_penalty", _search_info.reverse_penalty );
nav2_util::declare_parameter_if_not_declared(
node, name + ".change_penalty", rclcpp::ParameterValue( 0.0 ) );
node->get_parameter( name + ".change_penalty", _search_info.change_penalty );
nav2_util::declare_parameter_if_not_declared(
node, name + ".non_straight_penalty", rclcpp::ParameterValue( 1.2 ) );
node->get_parameter( name + ".non_straight_penalty", _search_info.non_straight_penalty );
nav2_util::declare_parameter_if_not_declared(
node, name + ".cost_penalty", rclcpp::ParameterValue( 2.0 ) );
node->get_parameter( name + ".cost_penalty", _search_info.cost_penalty );
nav2_util::declare_parameter_if_not_declared(
node, name + ".retrospective_penalty", rclcpp::ParameterValue( 0.015 ) );
node->get_parameter( name + ".retrospective_penalty", _search_info.retrospective_penalty );
nav2_util::declare_parameter_if_not_declared(
node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue( 3.5 ) );
node->get_parameter( name + ".analytic_expansion_ratio", _search_info.analytic_expansion_ratio );
nav2_util::declare_parameter_if_not_declared(
node, name + ".analytic_expansion_max_length", rclcpp::ParameterValue( 3.0 ) );
node->get_parameter( name + ".analytic_expansion_max_length", analytic_expansion_max_length_m );
_search_info.analytic_expansion_max_length =
analytic_expansion_max_length_m / _costmap->getResolution( );
nav2_util::declare_parameter_if_not_declared(
node, name + ".max_planning_time", rclcpp::ParameterValue( 5.0 ) );
node->get_parameter( name + ".max_planning_time", _max_planning_time );
nav2_util::declare_parameter_if_not_declared(
node, name + ".lookup_table_size", rclcpp::ParameterValue( 20.0 ) );
node->get_parameter( name + ".lookup_table_size", _lookup_table_size );
nav2_util::declare_parameter_if_not_declared(
node, name + ".motion_model_for_search", rclcpp::ParameterValue( std::string( "DUBIN" ) ) );
node->get_parameter( name + ".motion_model_for_search", _motion_model_for_search );
_motion_model = fromString( _motion_model_for_search );
if ( _motion_model == MotionModel::UNKNOWN ) {
RCLCPP_WARN(
_logger,
"Unable to get MotionModel search type. Given '%s', "
"valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP, STATE_LATTICE.",
_motion_model_for_search.c_str( ) );
}
if ( _max_iterations <= 0 ) {
RCLCPP_INFO(
_logger, "maximum iteration selected as <= 0, "
"disabling maximum iterations." );
_max_iterations = std::numeric_limits<int>::max( );
}
// convert to grid coordinates
if ( !_downsample_costmap ) {
_downsampling_factor = 1;
}
_search_info.minimum_turning_radius =
_minimum_turning_radius_global_coords / ( _costmap->getResolution( ) * _downsampling_factor );
_lookup_table_dim =
static_cast<float>( _lookup_table_size ) /
static_cast<float>( _costmap->getResolution( ) * _downsampling_factor );
// Make sure its a whole number
_lookup_table_dim = static_cast<float>( static_cast<int>( _lookup_table_dim ) );
// Make sure its an odd number
if ( static_cast<int>( _lookup_table_dim ) % 2 == 0 ) {
RCLCPP_INFO(
_logger,
"Even sized heuristic lookup table size set %f, increasing size by 1 to make odd",
_lookup_table_dim );
_lookup_table_dim += 1.0;
}
// Initialize collision checker
_collision_checker = GridCollisionChecker( _costmap, _angle_quantizations );
_collision_checker.setFootprint(
_costmap_ros->getRobotFootprint( ),
_costmap_ros->getUseRadius( ),
findCircumscribedCost( _costmap_ros ) );
// Initialize A* template
_a_star = std::make_unique<AStarAlgorithm<NodeHybrid>>( _motion_model, _search_info );
_a_star->initialize(
_allow_unknown,
_max_iterations,
std::numeric_limits<int>::max( ),
_max_planning_time,
_lookup_table_dim,
_angle_quantizations );
// Initialize path smoother
if ( smooth_path ) {
SmootherParams params;
params.get( node, name );
_smoother = std::make_unique<Smoother>( params );
_smoother->initialize( _minimum_turning_radius_global_coords );
}
// Initialize costmap downsampler
if ( _downsample_costmap && _downsampling_factor > 1 ) {
_costmap_downsampler = std::make_unique<CostmapDownsampler>( );
std::string topic_name = "downsampled_costmap";
_costmap_downsampler->on_configure(
node, _global_frame, topic_name, _costmap, _downsampling_factor );
}
_raw_plan_publisher = node->create_publisher<nav_msgs::msg::Path>( "unsmoothed_plan", 1 );
RCLCPP_INFO(
_logger, "Configured plugin %s of type SmacPlannerHybrid with "
"maximum iterations %i, and %s. Using motion model: %s.",
_name.c_str( ), _max_iterations,
_allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal",
toString( _motion_model ).c_str( ) );
}
214 void SmacPlannerHybrid::activate( )
{
RCLCPP_INFO(
_logger, "Activating plugin %s of type SmacPlannerHybrid",
_name.c_str( ) );
_raw_plan_publisher->on_activate( );
if ( _costmap_downsampler ) {
_costmap_downsampler->on_activate( );
}
auto node = _node.lock( );
// Add callback for dynamic parameters
_dyn_params_handler = node->add_on_set_parameters_callback(
std::bind( &SmacPlannerHybrid::dynamicParametersCallback, this, _1 ) );
}
229 void SmacPlannerHybrid::deactivate( )
{
RCLCPP_INFO(
_logger, "Deactivating plugin %s of type SmacPlannerHybrid",
_name.c_str( ) );
_raw_plan_publisher->on_deactivate( );
if ( _costmap_downsampler ) {
_costmap_downsampler->on_deactivate( );
}
_dyn_params_handler.reset( );
}
241 void SmacPlannerHybrid::cleanup( )
{
RCLCPP_INFO(
_logger, "Cleaning up plugin %s of type SmacPlannerHybrid",
_name.c_str( ) );
_a_star.reset( );
_smoother.reset( );
if ( _costmap_downsampler ) {
_costmap_downsampler->on_cleanup( );
_costmap_downsampler.reset( );
}
_raw_plan_publisher.reset( );
}
255 nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
256 const geometry_msgs::msg::PoseStamped & start,
257 const geometry_msgs::msg::PoseStamped & goal )
{
std::lock_guard<std::mutex> lock_reinit( _mutex );
steady_clock::time_point a = steady_clock::now( );
std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock( *( _costmap->getMutex( ) ) );
// Downsample costmap, if required
nav2_costmap_2d::Costmap2D * costmap = _costmap;
if ( _costmap_downsampler ) {
costmap = _costmap_downsampler->downsample( _downsampling_factor );
_collision_checker.setCostmap( costmap );
}
// Set collision checker and costmap information
_a_star->setCollisionChecker( &_collision_checker );
// Set starting point, in A* bin search coordinates
unsigned int mx, my;
costmap->worldToMap( start.pose.position.x, start.pose.position.y, mx, my );
double orientation_bin = tf2::getYaw( start.pose.orientation ) / _angle_bin_size;
while ( orientation_bin < 0.0 ) {
orientation_bin += static_cast<float>( _angle_quantizations );
}
// This is needed to handle precision issues
if ( orientation_bin >= static_cast<float>( _angle_quantizations ) ) {
orientation_bin -= static_cast<float>( _angle_quantizations );
}
unsigned int orientation_bin_id = static_cast<unsigned int>( floor( orientation_bin ) );
_a_star->setStart( mx, my, orientation_bin_id );
// Set goal point, in A* bin search coordinates
costmap->worldToMap( goal.pose.position.x, goal.pose.position.y, mx, my );
orientation_bin = tf2::getYaw( goal.pose.orientation ) / _angle_bin_size;
while ( orientation_bin < 0.0 ) {
orientation_bin += static_cast<float>( _angle_quantizations );
}
// This is needed to handle precision issues
if ( orientation_bin >= static_cast<float>( _angle_quantizations ) ) {
orientation_bin -= static_cast<float>( _angle_quantizations );
}
orientation_bin_id = static_cast<unsigned int>( floor( orientation_bin ) );
_a_star->setGoal( mx, my, orientation_bin_id );
// Setup message
nav_msgs::msg::Path plan;
plan.header.stamp = _clock->now( );
plan.header.frame_id = _global_frame;
geometry_msgs::msg::PoseStamped pose;
pose.header = plan.header;
pose.pose.position.z = 0.0;
pose.pose.orientation.x = 0.0;
pose.pose.orientation.y = 0.0;
pose.pose.orientation.z = 0.0;
pose.pose.orientation.w = 1.0;
// Compute plan
NodeHybrid::CoordinateVector path;
int num_iterations = 0;
std::string error;
try {
if ( !_a_star->createPath( path, num_iterations, 0.0 ) ) {
if ( num_iterations < _a_star->getMaxIterations( ) ) {
error = std::string( "no valid path found" );
} else {
error = std::string( "exceeded maximum iterations" );
}
}
} catch ( const std::runtime_error & e ) {
error = "invalid use: ";
error += e.what( );
}
if ( !error.empty( ) ) {
RCLCPP_WARN(
_logger,
"%s: failed to create plan, %s.",
_name.c_str( ), error.c_str( ) );
return plan;
}
// Convert to world coordinates
plan.poses.reserve( path.size( ) );
for ( int i = path.size( ) - 1; i >= 0; --i ) {
pose.pose = getWorldCoords( path[i].x, path[i].y, costmap );
pose.pose.orientation = getWorldOrientation( path[i].theta );
plan.poses.push_back( pose );
}
// Publish raw path for debug
if ( _raw_plan_publisher->get_subscription_count( ) > 0 ) {
_raw_plan_publisher->publish( plan );
}
// Find how much time we have left to do smoothing
steady_clock::time_point b = steady_clock::now( );
duration<double> time_span = duration_cast<duration<double>>( b - a );
double time_remaining = _max_planning_time - static_cast<double>( time_span.count( ) );
#ifdef BENCHMARK_TESTING
std::cout << "It took " << time_span.count( ) * 1000 <<
" milliseconds with " << num_iterations << " iterations." << std::endl;
#endif
// Smooth plan
if ( _smoother && num_iterations > 1 ) {
_smoother->smooth( plan, costmap, time_remaining );
}
#ifdef BENCHMARK_TESTING
steady_clock::time_point c = steady_clock::now( );
duration<double> time_span2 = duration_cast<duration<double>>( c - b );
std::cout << "It took " << time_span2.count( ) * 1000 <<
" milliseconds to smooth path." << std::endl;
#endif
return plan;
}
rcl_interfaces::msg::SetParametersResult
377 SmacPlannerHybrid::dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters )
{
rcl_interfaces::msg::SetParametersResult result;
std::lock_guard<std::mutex> lock_reinit( _mutex );
bool reinit_collision_checker = false;
bool reinit_a_star = false;
bool reinit_downsampler = false;
bool reinit_smoother = false;
for ( auto parameter : parameters ) {
const auto & type = parameter.get_type( );
const auto & name = parameter.get_name( );
if ( type == ParameterType::PARAMETER_DOUBLE ) {
if ( name == _name + ".max_planning_time" ) {
reinit_a_star = true;
_max_planning_time = parameter.as_double( );
} else if ( name == _name + ".lookup_table_size" ) {
reinit_a_star = true;
_lookup_table_size = parameter.as_double( );
} else if ( name == _name + ".minimum_turning_radius" ) {
reinit_a_star = true;
if ( _smoother ) {
reinit_smoother = true;
}
_minimum_turning_radius_global_coords = static_cast<float>( parameter.as_double( ) );
} else if ( name == _name + ".reverse_penalty" ) {
reinit_a_star = true;
_search_info.reverse_penalty = static_cast<float>( parameter.as_double( ) );
} else if ( name == _name + ".change_penalty" ) {
reinit_a_star = true;
_search_info.change_penalty = static_cast<float>( parameter.as_double( ) );
} else if ( name == _name + ".non_straight_penalty" ) {
reinit_a_star = true;
_search_info.non_straight_penalty = static_cast<float>( parameter.as_double( ) );
} else if ( name == _name + ".cost_penalty" ) {
reinit_a_star = true;
_search_info.cost_penalty = static_cast<float>( parameter.as_double( ) );
} else if ( name == _name + ".analytic_expansion_ratio" ) {
reinit_a_star = true;
_search_info.analytic_expansion_ratio = static_cast<float>( parameter.as_double( ) );
} else if ( name == _name + ".analytic_expansion_max_length" ) {
reinit_a_star = true;
_search_info.analytic_expansion_max_length =
static_cast<float>( parameter.as_double( ) ) / _costmap->getResolution( );
}
} else if ( type == ParameterType::PARAMETER_BOOL ) {
if ( name == _name + ".downsample_costmap" ) {
reinit_downsampler = true;
_downsample_costmap = parameter.as_bool( );
} else if ( name == _name + ".allow_unknown" ) {
reinit_a_star = true;
_allow_unknown = parameter.as_bool( );
} else if ( name == _name + ".cache_obstacle_heuristic" ) {
reinit_a_star = true;
_search_info.cache_obstacle_heuristic = parameter.as_bool( );
} else if ( name == _name + ".smooth_path" ) {
if ( parameter.as_bool( ) ) {
reinit_smoother = true;
} else {
_smoother.reset( );
}
}
} else if ( type == ParameterType::PARAMETER_INTEGER ) {
if ( name == _name + ".downsampling_factor" ) {
reinit_a_star = true;
reinit_downsampler = true;
_downsampling_factor = parameter.as_int( );
} else if ( name == _name + ".max_iterations" ) {
reinit_a_star = true;
_max_iterations = parameter.as_int( );
if ( _max_iterations <= 0 ) {
RCLCPP_INFO(
_logger, "maximum iteration selected as <= 0, "
"disabling maximum iterations." );
_max_iterations = std::numeric_limits<int>::max( );
}
} else if ( name == _name + ".angle_quantization_bins" ) {
reinit_collision_checker = true;
reinit_a_star = true;
int angle_quantizations = parameter.as_int( );
_angle_bin_size = 2.0 * M_PI / angle_quantizations;
_angle_quantizations = static_cast<unsigned int>( angle_quantizations );
}
} else if ( type == ParameterType::PARAMETER_STRING ) {
if ( name == _name + ".motion_model_for_search" ) {
reinit_a_star = true;
_motion_model = fromString( parameter.as_string( ) );
if ( _motion_model == MotionModel::UNKNOWN ) {
RCLCPP_WARN(
_logger,
"Unable to get MotionModel search type. Given '%s', "
"valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.",
_motion_model_for_search.c_str( ) );
}
}
}
}
// Re-init if needed with mutex lock ( to avoid re-init while creating a plan )
if ( reinit_a_star || reinit_downsampler || reinit_collision_checker || reinit_smoother ) {
// convert to grid coordinates
if ( !_downsample_costmap ) {
_downsampling_factor = 1;
}
_search_info.minimum_turning_radius =
_minimum_turning_radius_global_coords / ( _costmap->getResolution( ) * _downsampling_factor );
_lookup_table_dim =
static_cast<float>( _lookup_table_size ) /
static_cast<float>( _costmap->getResolution( ) * _downsampling_factor );
// Make sure its a whole number
_lookup_table_dim = static_cast<float>( static_cast<int>( _lookup_table_dim ) );
// Make sure its an odd number
if ( static_cast<int>( _lookup_table_dim ) % 2 == 0 ) {
RCLCPP_INFO(
_logger,
"Even sized heuristic lookup table size set %f, increasing size by 1 to make odd",
_lookup_table_dim );
_lookup_table_dim += 1.0;
}
// Re-Initialize A* template
if ( reinit_a_star ) {
_a_star = std::make_unique<AStarAlgorithm<NodeHybrid>>( _motion_model, _search_info );
_a_star->initialize(
_allow_unknown,
_max_iterations,
std::numeric_limits<int>::max( ),
_max_planning_time,
_lookup_table_dim,
_angle_quantizations );
}
// Re-Initialize costmap downsampler
if ( reinit_downsampler ) {
if ( _downsample_costmap && _downsampling_factor > 1 ) {
auto node = _node.lock( );
std::string topic_name = "downsampled_costmap";
_costmap_downsampler = std::make_unique<CostmapDownsampler>( );
_costmap_downsampler->on_configure(
node, _global_frame, topic_name, _costmap, _downsampling_factor );
}
}
// Re-Initialize collision checker
if ( reinit_collision_checker ) {
_collision_checker = GridCollisionChecker( _costmap, _angle_quantizations );
_collision_checker.setFootprint(
_costmap_ros->getRobotFootprint( ),
_costmap_ros->getUseRadius( ),
findCircumscribedCost( _costmap_ros ) );
}
// Re-Initialize smoother
if ( reinit_smoother ) {
auto node = _node.lock( );
SmootherParams params;
params.get( node, _name );
_smoother = std::make_unique<Smoother>( params );
_smoother->initialize( _minimum_turning_radius_global_coords );
}
}
result.successful = true;
return result;
}
} // namespace nav2_smac_planner
#include "pluginlib/class_list_macros.hpp"
549 PLUGINLIB_EXPORT_CLASS( nav2_smac_planner::SmacPlannerHybrid, nav2_core::GlobalPlanner )
1 // Copyright ( c ) 2021, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <string>
#include <memory>
#include <vector>
#include <algorithm>
#include <limits>
#include "Eigen/Core"
#include "nav2_smac_planner/smac_planner_lattice.hpp"
// #define BENCHMARK_TESTING
namespace nav2_smac_planner
{
using namespace std::chrono; // NOLINT
using rcl_interfaces::msg::ParameterType;
32 SmacPlannerLattice::SmacPlannerLattice( )
: _a_star( nullptr ),
_collision_checker( nullptr, 1 ),
_smoother( nullptr ),
_costmap( nullptr )
{
}
40 SmacPlannerLattice::~SmacPlannerLattice( )
{
RCLCPP_INFO(
_logger, "Destroying plugin %s of type SmacPlannerLattice",
_name.c_str( ) );
}
47 void SmacPlannerLattice::configure(
48 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
49 std::string name, std::shared_ptr<tf2_ros::Buffer>/*tf*/,
50 std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros )
{
_node = parent;
auto node = parent.lock( );
_logger = node->get_logger( );
_clock = node->get_clock( );
_costmap = costmap_ros->getCostmap( );
_costmap_ros = costmap_ros;
_name = name;
_global_frame = costmap_ros->getGlobalFrameID( );
_raw_plan_publisher = node->create_publisher<nav_msgs::msg::Path>( "unsmoothed_plan", 1 );
RCLCPP_INFO( _logger, "Configuring %s of type SmacPlannerLattice", name.c_str( ) );
// General planner params
double analytic_expansion_max_length_m;
bool smooth_path;
nav2_util::declare_parameter_if_not_declared(
node, name + ".allow_unknown", rclcpp::ParameterValue( true ) );
node->get_parameter( name + ".allow_unknown", _allow_unknown );
nav2_util::declare_parameter_if_not_declared(
node, name + ".max_iterations", rclcpp::ParameterValue( 1000000 ) );
node->get_parameter( name + ".max_iterations", _max_iterations );
nav2_util::declare_parameter_if_not_declared(
node, name + ".smooth_path", rclcpp::ParameterValue( true ) );
node->get_parameter( name + ".smooth_path", smooth_path );
// Default to a well rounded model: 16 bin, 0.4m turning radius, ackermann model
nav2_util::declare_parameter_if_not_declared(
node, name + ".lattice_filepath", rclcpp::ParameterValue(
ament_index_cpp::get_package_share_directory( "nav2_smac_planner" ) +
"/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann/output.json" ) );
node->get_parameter( name + ".lattice_filepath", _search_info.lattice_filepath );
nav2_util::declare_parameter_if_not_declared(
node, name + ".cache_obstacle_heuristic", rclcpp::ParameterValue( false ) );
node->get_parameter( name + ".cache_obstacle_heuristic", _search_info.cache_obstacle_heuristic );
nav2_util::declare_parameter_if_not_declared(
node, name + ".reverse_penalty", rclcpp::ParameterValue( 2.0 ) );
node->get_parameter( name + ".reverse_penalty", _search_info.reverse_penalty );
nav2_util::declare_parameter_if_not_declared(
node, name + ".change_penalty", rclcpp::ParameterValue( 0.05 ) );
node->get_parameter( name + ".change_penalty", _search_info.change_penalty );
nav2_util::declare_parameter_if_not_declared(
node, name + ".non_straight_penalty", rclcpp::ParameterValue( 1.05 ) );
node->get_parameter( name + ".non_straight_penalty", _search_info.non_straight_penalty );
nav2_util::declare_parameter_if_not_declared(
node, name + ".cost_penalty", rclcpp::ParameterValue( 2.0 ) );
node->get_parameter( name + ".cost_penalty", _search_info.cost_penalty );
nav2_util::declare_parameter_if_not_declared(
node, name + ".retrospective_penalty", rclcpp::ParameterValue( 0.015 ) );
node->get_parameter( name + ".retrospective_penalty", _search_info.retrospective_penalty );
nav2_util::declare_parameter_if_not_declared(
node, name + ".rotation_penalty", rclcpp::ParameterValue( 5.0 ) );
node->get_parameter( name + ".rotation_penalty", _search_info.rotation_penalty );
nav2_util::declare_parameter_if_not_declared(
node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue( 3.5 ) );
node->get_parameter( name + ".analytic_expansion_ratio", _search_info.analytic_expansion_ratio );
nav2_util::declare_parameter_if_not_declared(
node, name + ".analytic_expansion_max_length", rclcpp::ParameterValue( 3.0 ) );
node->get_parameter( name + ".analytic_expansion_max_length", analytic_expansion_max_length_m );
_search_info.analytic_expansion_max_length =
analytic_expansion_max_length_m / _costmap->getResolution( );
nav2_util::declare_parameter_if_not_declared(
node, name + ".max_planning_time", rclcpp::ParameterValue( 5.0 ) );
node->get_parameter( name + ".max_planning_time", _max_planning_time );
nav2_util::declare_parameter_if_not_declared(
node, name + ".lookup_table_size", rclcpp::ParameterValue( 20.0 ) );
node->get_parameter( name + ".lookup_table_size", _lookup_table_size );
nav2_util::declare_parameter_if_not_declared(
node, name + ".allow_reverse_expansion", rclcpp::ParameterValue( false ) );
node->get_parameter( name + ".allow_reverse_expansion", _search_info.allow_reverse_expansion );
_metadata = LatticeMotionTable::getLatticeMetadata( _search_info.lattice_filepath );
_search_info.minimum_turning_radius =
_metadata.min_turning_radius / ( _costmap->getResolution( ) );
_motion_model = MotionModel::STATE_LATTICE;
if ( _max_iterations <= 0 ) {
RCLCPP_INFO(
_logger, "maximum iteration selected as <= 0, "
"disabling maximum iterations." );
_max_iterations = std::numeric_limits<int>::max( );
}
float lookup_table_dim =
static_cast<float>( _lookup_table_size ) /
static_cast<float>( _costmap->getResolution( ) );
// Make sure its a whole number
lookup_table_dim = static_cast<float>( static_cast<int>( lookup_table_dim ) );
// Make sure its an odd number
if ( static_cast<int>( lookup_table_dim ) % 2 == 0 ) {
RCLCPP_INFO(
_logger,
"Even sized heuristic lookup table size set %f, increasing size by 1 to make odd",
lookup_table_dim );
lookup_table_dim += 1.0;
}
// Initialize collision checker using 72 evenly sized bins instead of the lattice
// heading angles. This is done so that we have precomputed angles every 5 degrees.
// If we used the sparse lattice headings ( usually 16 ), then when we attempt to collision
// check for intermediary points of the primitives, we're forced to round to one of the 16
// increments causing "wobbly" checks that could cause larger robots to virtually show collisions
// in valid configurations. This approximation helps to bound orientation error for all checks
// in exchange for slight inaccuracies in the collision headings in terminal search states.
_collision_checker = GridCollisionChecker( _costmap, 72u );
_collision_checker.setFootprint(
costmap_ros->getRobotFootprint( ),
costmap_ros->getUseRadius( ),
findCircumscribedCost( costmap_ros ) );
// Initialize A* template
_a_star = std::make_unique<AStarAlgorithm<NodeLattice>>( _motion_model, _search_info );
_a_star->initialize(
_allow_unknown,
_max_iterations,
std::numeric_limits<int>::max( ),
_max_planning_time,
lookup_table_dim,
_metadata.number_of_headings );
// Initialize path smoother
if ( smooth_path ) {
SmootherParams params;
params.get( node, name );
_smoother = std::make_unique<Smoother>( params );
_smoother->initialize( _metadata.min_turning_radius );
}
RCLCPP_INFO(
_logger, "Configured plugin %s of type SmacPlannerLattice with "
"maximum iterations %i, "
"and %s. Using motion model: %s. State lattice file: %s.",
_name.c_str( ), _max_iterations,
_allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal",
toString( _motion_model ).c_str( ), _search_info.lattice_filepath.c_str( ) );
}
192 void SmacPlannerLattice::activate( )
{
RCLCPP_INFO(
_logger, "Activating plugin %s of type SmacPlannerLattice",
_name.c_str( ) );
_raw_plan_publisher->on_activate( );
auto node = _node.lock( );
// Add callback for dynamic parameters
_dyn_params_handler = node->add_on_set_parameters_callback(
std::bind( &SmacPlannerLattice::dynamicParametersCallback, this, std::placeholders::_1 ) );
}
204 void SmacPlannerLattice::deactivate( )
{
RCLCPP_INFO(
_logger, "Deactivating plugin %s of type SmacPlannerLattice",
_name.c_str( ) );
_raw_plan_publisher->on_deactivate( );
_dyn_params_handler.reset( );
}
213 void SmacPlannerLattice::cleanup( )
{
RCLCPP_INFO(
_logger, "Cleaning up plugin %s of type SmacPlannerLattice",
_name.c_str( ) );
_a_star.reset( );
_smoother.reset( );
_raw_plan_publisher.reset( );
}
223 nav_msgs::msg::Path SmacPlannerLattice::createPlan(
224 const geometry_msgs::msg::PoseStamped & start,
225 const geometry_msgs::msg::PoseStamped & goal )
{
std::lock_guard<std::mutex> lock_reinit( _mutex );
steady_clock::time_point a = steady_clock::now( );
std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock( *( _costmap->getMutex( ) ) );
// Set collision checker and costmap information
_a_star->setCollisionChecker( &_collision_checker );
// Set starting point, in A* bin search coordinates
unsigned int mx, my;
_costmap->worldToMap( start.pose.position.x, start.pose.position.y, mx, my );
_a_star->setStart(
mx, my,
NodeLattice::motion_table.getClosestAngularBin( tf2::getYaw( start.pose.orientation ) ) );
// Set goal point, in A* bin search coordinates
_costmap->worldToMap( goal.pose.position.x, goal.pose.position.y, mx, my );
_a_star->setGoal(
mx, my,
NodeLattice::motion_table.getClosestAngularBin( tf2::getYaw( goal.pose.orientation ) ) );
// Setup message
nav_msgs::msg::Path plan;
plan.header.stamp = _clock->now( );
plan.header.frame_id = _global_frame;
geometry_msgs::msg::PoseStamped pose;
pose.header = plan.header;
pose.pose.position.z = 0.0;
pose.pose.orientation.x = 0.0;
pose.pose.orientation.y = 0.0;
pose.pose.orientation.z = 0.0;
pose.pose.orientation.w = 1.0;
// Compute plan
NodeLattice::CoordinateVector path;
int num_iterations = 0;
std::string error;
try {
if ( !_a_star->createPath( path, num_iterations, 0 /*no tolerance*/ ) ) {
if ( num_iterations < _a_star->getMaxIterations( ) ) {
error = std::string( "no valid path found" );
} else {
error = std::string( "exceeded maximum iterations" );
}
}
} catch ( const std::runtime_error & e ) {
error = "invalid use: ";
error += e.what( );
}
if ( !error.empty( ) ) {
RCLCPP_WARN(
_logger,
"%s: failed to create plan, %s.",
_name.c_str( ), error.c_str( ) );
return plan;
}
// Convert to world coordinates
plan.poses.reserve( path.size( ) );
geometry_msgs::msg::PoseStamped last_pose = pose;
for ( int i = path.size( ) - 1; i >= 0; --i ) {
pose.pose = getWorldCoords( path[i].x, path[i].y, _costmap );
pose.pose.orientation = getWorldOrientation( path[i].theta );
if ( fabs( pose.pose.position.x - last_pose.pose.position.x ) < 1e-4 &&
fabs( pose.pose.position.y - last_pose.pose.position.y ) < 1e-4 &&
fabs( tf2::getYaw( pose.pose.orientation ) - tf2::getYaw( last_pose.pose.orientation ) ) < 1e-4 )
{
RCLCPP_DEBUG(
_logger,
"Removed a path from the path due to replication. "
"Make sure your minimum control set does not contain duplicate values!" );
continue;
}
last_pose = pose;
plan.poses.push_back( pose );
}
// Publish raw path for debug
if ( _raw_plan_publisher->get_subscription_count( ) > 0 ) {
_raw_plan_publisher->publish( plan );
}
// Find how much time we have left to do smoothing
steady_clock::time_point b = steady_clock::now( );
duration<double> time_span = duration_cast<duration<double>>( b - a );
double time_remaining = _max_planning_time - static_cast<double>( time_span.count( ) );
#ifdef BENCHMARK_TESTING
std::cout << "It took " << time_span.count( ) * 1000 <<
" milliseconds with " << num_iterations << " iterations." << std::endl;
#endif
// Smooth plan
if ( _smoother && num_iterations > 1 ) {
_smoother->smooth( plan, _costmap, time_remaining );
}
#ifdef BENCHMARK_TESTING
steady_clock::time_point c = steady_clock::now( );
duration<double> time_span2 = duration_cast<duration<double>>( c - b );
std::cout << "It took " << time_span2.count( ) * 1000 <<
" milliseconds to smooth path." << std::endl;
#endif
return plan;
}
rcl_interfaces::msg::SetParametersResult
336 SmacPlannerLattice::dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters )
{
rcl_interfaces::msg::SetParametersResult result;
std::lock_guard<std::mutex> lock_reinit( _mutex );
bool reinit_a_star = false;
bool reinit_smoother = false;
for ( auto parameter : parameters ) {
const auto & type = parameter.get_type( );
const auto & name = parameter.get_name( );
if ( type == ParameterType::PARAMETER_DOUBLE ) {
if ( name == _name + ".max_planning_time" ) {
reinit_a_star = true;
_max_planning_time = parameter.as_double( );
} else if ( name == _name + ".lookup_table_size" ) {
reinit_a_star = true;
_lookup_table_size = parameter.as_double( );
} else if ( name == _name + ".reverse_penalty" ) {
reinit_a_star = true;
_search_info.reverse_penalty = static_cast<float>( parameter.as_double( ) );
} else if ( name == _name + ".change_penalty" ) {
reinit_a_star = true;
_search_info.change_penalty = static_cast<float>( parameter.as_double( ) );
} else if ( name == _name + ".non_straight_penalty" ) {
reinit_a_star = true;
_search_info.non_straight_penalty = static_cast<float>( parameter.as_double( ) );
} else if ( name == _name + ".cost_penalty" ) {
reinit_a_star = true;
_search_info.cost_penalty = static_cast<float>( parameter.as_double( ) );
} else if ( name == _name + ".rotation_penalty" ) {
reinit_a_star = true;
_search_info.rotation_penalty = static_cast<float>( parameter.as_double( ) );
} else if ( name == _name + ".analytic_expansion_ratio" ) {
reinit_a_star = true;
_search_info.analytic_expansion_ratio = static_cast<float>( parameter.as_double( ) );
} else if ( name == _name + ".analytic_expansion_max_length" ) {
reinit_a_star = true;
_search_info.analytic_expansion_max_length =
static_cast<float>( parameter.as_double( ) ) / _costmap->getResolution( );
}
} else if ( type == ParameterType::PARAMETER_BOOL ) {
if ( name == _name + ".allow_unknown" ) {
reinit_a_star = true;
_allow_unknown = parameter.as_bool( );
} else if ( name == _name + ".cache_obstacle_heuristic" ) {
reinit_a_star = true;
_search_info.cache_obstacle_heuristic = parameter.as_bool( );
} else if ( name == _name + ".allow_reverse_expansion" ) {
reinit_a_star = true;
_search_info.allow_reverse_expansion = parameter.as_bool( );
} else if ( name == _name + ".smooth_path" ) {
if ( parameter.as_bool( ) ) {
reinit_smoother = true;
} else {
_smoother.reset( );
}
}
} else if ( type == ParameterType::PARAMETER_INTEGER ) {
if ( name == _name + ".max_iterations" ) {
reinit_a_star = true;
_max_iterations = parameter.as_int( );
if ( _max_iterations <= 0 ) {
RCLCPP_INFO(
_logger, "maximum iteration selected as <= 0, "
"disabling maximum iterations." );
_max_iterations = std::numeric_limits<int>::max( );
}
}
} else if ( type == ParameterType::PARAMETER_STRING ) {
if ( name == _name + ".lattice_filepath" ) {
reinit_a_star = true;
if ( _smoother ) {
reinit_smoother = true;
}
_search_info.lattice_filepath = parameter.as_string( );
_metadata = LatticeMotionTable::getLatticeMetadata( _search_info.lattice_filepath );
_search_info.minimum_turning_radius =
_metadata.min_turning_radius / ( _costmap->getResolution( ) );
}
}
}
// Re-init if needed with mutex lock ( to avoid re-init while creating a plan )
if ( reinit_a_star || reinit_smoother ) {
// convert to grid coordinates
_search_info.minimum_turning_radius =
_metadata.min_turning_radius / ( _costmap->getResolution( ) );
float lookup_table_dim =
static_cast<float>( _lookup_table_size ) /
static_cast<float>( _costmap->getResolution( ) );
// Make sure its a whole number
lookup_table_dim = static_cast<float>( static_cast<int>( lookup_table_dim ) );
// Make sure its an odd number
if ( static_cast<int>( lookup_table_dim ) % 2 == 0 ) {
RCLCPP_INFO(
_logger,
"Even sized heuristic lookup table size set %f, increasing size by 1 to make odd",
lookup_table_dim );
lookup_table_dim += 1.0;
}
// Re-Initialize smoother
if ( reinit_smoother ) {
auto node = _node.lock( );
SmootherParams params;
params.get( node, _name );
_smoother = std::make_unique<Smoother>( params );
_smoother->initialize( _metadata.min_turning_radius );
}
// Re-Initialize A* template
if ( reinit_a_star ) {
_a_star = std::make_unique<AStarAlgorithm<NodeLattice>>( _motion_model, _search_info );
_a_star->initialize(
_allow_unknown,
_max_iterations,
std::numeric_limits<int>::max( ),
_max_planning_time,
lookup_table_dim,
_metadata.number_of_headings );
}
}
result.successful = true;
return result;
}
} // namespace nav2_smac_planner
#include "pluginlib/class_list_macros.hpp"
470 PLUGINLIB_EXPORT_CLASS( nav2_smac_planner::SmacPlannerLattice, nav2_core::GlobalPlanner )
1 // Copyright ( c ) 2021, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <ompl/base/ScopedState.h>
#include <ompl/base/spaces/DubinsStateSpace.h>
#include <vector>
#include <memory>
#include "nav2_smac_planner/smoother.hpp"
namespace nav2_smac_planner
{
using namespace nav2_util::geometry_utils; // NOLINT
using namespace std::chrono; // NOLINT
26 Smoother::Smoother( const SmootherParams & params )
{
tolerance_ = params.tolerance_;
max_its_ = params.max_its_;
data_w_ = params.w_data_;
smooth_w_ = params.w_smooth_;
is_holonomic_ = params.holonomic_;
do_refinement_ = params.do_refinement_;
}
36 void Smoother::initialize( const double & min_turning_radius )
{
min_turning_rad_ = min_turning_radius;
state_space_ = std::make_unique<ompl::base::DubinsStateSpace>( min_turning_rad_ );
}
42 bool Smoother::smooth(
43 nav_msgs::msg::Path & path,
44 const nav2_costmap_2d::Costmap2D * costmap,
const double & max_time )
{
// by-pass path orientations approximation when skipping smac smoother
if ( max_its_ == 0 ) {
return false;
}
refinement_ctr_ = 0;
steady_clock::time_point start = steady_clock::now( );
double time_remaining = max_time;
bool success = true, reversing_segment;
nav_msgs::msg::Path curr_path_segment;
curr_path_segment.header = path.header;
std::vector<PathSegment> path_segments = findDirectionalPathSegments( path );
for ( unsigned int i = 0; i != path_segments.size( ); i++ ) {
if ( path_segments[i].end - path_segments[i].start > 10 ) {
// Populate path segment
curr_path_segment.poses.clear( );
std::copy(
path.poses.begin( ) + path_segments[i].start,
path.poses.begin( ) + path_segments[i].end + 1,
std::back_inserter( curr_path_segment.poses ) );
// Make sure we're still able to smooth with time remaining
steady_clock::time_point now = steady_clock::now( );
time_remaining = max_time - duration_cast<duration<double>>( now - start ).count( );
// Smooth path segment naively
const geometry_msgs::msg::Pose start_pose = curr_path_segment.poses.front( ).pose;
const geometry_msgs::msg::Pose goal_pose = curr_path_segment.poses.back( ).pose;
bool local_success =
smoothImpl( curr_path_segment, reversing_segment, costmap, time_remaining );
success = success && local_success;
// Enforce boundary conditions
if ( !is_holonomic_ && local_success ) {
enforceStartBoundaryConditions( start_pose, curr_path_segment, costmap, reversing_segment );
enforceEndBoundaryConditions( goal_pose, curr_path_segment, costmap, reversing_segment );
}
// Assemble the path changes to the main path
std::copy(
curr_path_segment.poses.begin( ),
curr_path_segment.poses.end( ),
path.poses.begin( ) + path_segments[i].start );
}
}
return success;
}
97 bool Smoother::smoothImpl(
98 nav_msgs::msg::Path & path,
99 bool & reversing_segment,
100 const nav2_costmap_2d::Costmap2D * costmap,
const double & max_time )
{
steady_clock::time_point a = steady_clock::now( );
rclcpp::Duration max_dur = rclcpp::Duration::from_seconds( max_time );
int its = 0;
double change = tolerance_;
const unsigned int & path_size = path.poses.size( );
double x_i, y_i, y_m1, y_ip1, y_i_org;
unsigned int mx, my;
nav_msgs::msg::Path new_path = path;
nav_msgs::msg::Path last_path = path;
while ( change >= tolerance_ ) {
its += 1;
change = 0.0;
// Make sure the smoothing function will converge
if ( its >= max_its_ ) {
RCLCPP_DEBUG(
rclcpp::get_logger( "SmacPlannerSmoother" ),
"Number of iterations has exceeded limit of %i.", max_its_ );
path = last_path;
updateApproximatePathOrientations( path, reversing_segment );
return false;
}
// Make sure still have time left to process
steady_clock::time_point b = steady_clock::now( );
rclcpp::Duration timespan( duration_cast<duration<double>>( b - a ) );
if ( timespan > max_dur ) {
RCLCPP_DEBUG(
rclcpp::get_logger( "SmacPlannerSmoother" ),
"Smoothing time exceeded allowed duration of %0.2f.", max_time );
path = last_path;
updateApproximatePathOrientations( path, reversing_segment );
return false;
}
for ( unsigned int i = 1; i != path_size - 1; i++ ) {
for ( unsigned int j = 0; j != 2; j++ ) {
x_i = getFieldByDim( path.poses[i], j );
y_i = getFieldByDim( new_path.poses[i], j );
y_m1 = getFieldByDim( new_path.poses[i - 1], j );
y_ip1 = getFieldByDim( new_path.poses[i + 1], j );
y_i_org = y_i;
// Smooth based on local 3 point neighborhood and original data locations
y_i += data_w_ * ( x_i - y_i ) + smooth_w_ * ( y_ip1 + y_m1 - ( 2.0 * y_i ) );
setFieldByDim( new_path.poses[i], j, y_i );
change += abs( y_i - y_i_org );
}
// validate update is admissible, only checks cost if a valid costmap pointer is provided
float cost = 0.0;
if ( costmap ) {
costmap->worldToMap(
getFieldByDim( new_path.poses[i], 0 ),
getFieldByDim( new_path.poses[i], 1 ),
mx, my );
cost = static_cast<float>( costmap->getCost( mx, my ) );
}
if ( cost > MAX_NON_OBSTACLE && cost != UNKNOWN ) {
RCLCPP_DEBUG(
rclcpp::get_logger( "SmacPlannerSmoother" ),
"Smoothing process resulted in an infeasible collision. "
"Returning the last path before the infeasibility was introduced." );
path = last_path;
updateApproximatePathOrientations( path, reversing_segment );
return false;
}
}
last_path = new_path;
}
// Lets do additional refinement, it shouldn't take more than a couple milliseconds
// but really puts the path quality over the top.
if ( do_refinement_ && refinement_ctr_ < 4 ) {
refinement_ctr_++;
smoothImpl( new_path, reversing_segment, costmap, max_time );
}
updateApproximatePathOrientations( new_path, reversing_segment );
path = new_path;
return true;
}
191 double Smoother::getFieldByDim(
192 const geometry_msgs::msg::PoseStamped & msg, const unsigned int & dim )
{
if ( dim == 0 ) {
return msg.pose.position.x;
} else if ( dim == 1 ) {
return msg.pose.position.y;
} else {
return msg.pose.position.z;
}
}
203 void Smoother::setFieldByDim(
204 geometry_msgs::msg::PoseStamped & msg, const unsigned int dim,
const double & value )
{
if ( dim == 0 ) {
msg.pose.position.x = value;
} else if ( dim == 1 ) {
msg.pose.position.y = value;
} else {
msg.pose.position.z = value;
}
}
216 std::vector<PathSegment> Smoother::findDirectionalPathSegments( const nav_msgs::msg::Path & path )
{
std::vector<PathSegment> segments;
PathSegment curr_segment;
curr_segment.start = 0;
// If holonomic, no directional changes and
// may have abrupt angular changes from naive grid search
if ( is_holonomic_ ) {
curr_segment.end = path.poses.size( ) - 1;
segments.push_back( curr_segment );
return segments;
}
// Iterating through the path to determine the position of the cusp
for ( unsigned int idx = 1; idx < path.poses.size( ) - 1; ++idx ) {
// We have two vectors for the dot product OA and AB. Determining the vectors.
double oa_x = path.poses[idx].pose.position.x -
path.poses[idx - 1].pose.position.x;
double oa_y = path.poses[idx].pose.position.y -
path.poses[idx - 1].pose.position.y;
double ab_x = path.poses[idx + 1].pose.position.x -
path.poses[idx].pose.position.x;
double ab_y = path.poses[idx + 1].pose.position.y -
path.poses[idx].pose.position.y;
// Checking for the existance of cusp, in the path, using the dot product.
double dot_product = ( oa_x * ab_x ) + ( oa_y * ab_y );
if ( dot_product < 0.0 ) {
curr_segment.end = idx;
segments.push_back( curr_segment );
curr_segment.start = idx;
}
// Checking for the existance of a differential rotation in place.
double cur_theta = tf2::getYaw( path.poses[idx].pose.orientation );
double next_theta = tf2::getYaw( path.poses[idx + 1].pose.orientation );
double dtheta = angles::shortest_angular_distance( cur_theta, next_theta );
if ( fabs( ab_x ) < 1e-4 && fabs( ab_y ) < 1e-4 && fabs( dtheta ) > 1e-4 ) {
curr_segment.end = idx;
segments.push_back( curr_segment );
curr_segment.start = idx;
}
}
curr_segment.end = path.poses.size( ) - 1;
segments.push_back( curr_segment );
return segments;
}
266 void Smoother::updateApproximatePathOrientations(
267 nav_msgs::msg::Path & path,
268 bool & reversing_segment )
{
double dx, dy, theta, pt_yaw;
reversing_segment = false;
// Find if this path segment is in reverse
dx = path.poses[2].pose.position.x - path.poses[1].pose.position.x;
dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y;
theta = atan2( dy, dx );
pt_yaw = tf2::getYaw( path.poses[1].pose.orientation );
if ( !is_holonomic_ && fabs( angles::shortest_angular_distance( pt_yaw, theta ) ) > M_PI_2 ) {
reversing_segment = true;
}
// Find the angle relative the path position vectors
for ( unsigned int i = 0; i != path.poses.size( ) - 1; i++ ) {
dx = path.poses[i + 1].pose.position.x - path.poses[i].pose.position.x;
dy = path.poses[i + 1].pose.position.y - path.poses[i].pose.position.y;
theta = atan2( dy, dx );
// If points are overlapping, pass
if ( fabs( dx ) < 1e-4 && fabs( dy ) < 1e-4 ) {
continue;
}
// Flip the angle if this path segment is in reverse
if ( reversing_segment ) {
theta += M_PI; // orientationAroundZAxis will normalize
}
path.poses[i].pose.orientation = orientationAroundZAxis( theta );
}
}
302 unsigned int Smoother::findShortestBoundaryExpansionIdx(
303 const BoundaryExpansions & boundary_expansions )
{
// Check which is valid with the minimum integrated length such that
// shorter end-points away that are infeasible to achieve without
// a loop-de-loop are punished
double min_length = 1e9;
int shortest_boundary_expansion_idx = 1e9;
for ( unsigned int idx = 0; idx != boundary_expansions.size( ); idx++ ) {
if ( boundary_expansions[idx].expansion_path_length<min_length &&
!boundary_expansions[idx].in_collision &&
boundary_expansions[idx].path_end_idx>0.0 &&
boundary_expansions[idx].expansion_path_length > 0.0 )
{
min_length = boundary_expansions[idx].expansion_path_length;
shortest_boundary_expansion_idx = idx;
}
}
return shortest_boundary_expansion_idx;
}
324 void Smoother::findBoundaryExpansion(
325 const geometry_msgs::msg::Pose & start,
326 const geometry_msgs::msg::Pose & end,
BoundaryExpansion & expansion,
328 const nav2_costmap_2d::Costmap2D * costmap )
{
static ompl::base::ScopedState<> from( state_space_ ), to( state_space_ ), s( state_space_ );
from[0] = start.position.x;
from[1] = start.position.y;
from[2] = tf2::getYaw( start.orientation );
to[0] = end.position.x;
to[1] = end.position.y;
to[2] = tf2::getYaw( end.orientation );
double d = state_space_->distance( from( ), to( ) );
// If this path is too long compared to the original, then this is probably
// a loop-de-loop, treat as invalid as to not deviate too far from the original path.
// 2.0 selected from prinicipled choice of boundary test points
// r, 2 * r, r * PI, and 2 * PI * r. If there is a loop, it will be
// approximately 2 * PI * r, which is 2 * PI > r, PI > 2 * r, and 2 > r * PI.
// For all but the last backup test point, a loop would be approximately
// 2x greater than any of the selections.
if ( d > 2.0 * expansion.original_path_length ) {
return;
}
std::vector<double> reals;
double theta( 0.0 ), x( 0.0 ), y( 0.0 );
double x_m = start.position.x;
double y_m = start.position.y;
// Get intermediary poses
for ( double i = 0; i <= expansion.path_end_idx; i++ ) {
state_space_->interpolate( from( ), to( ), i / expansion.path_end_idx, s( ) );
reals = s.reals( );
// Make sure in range [0, 2PI )
theta = ( reals[2] < 0.0 ) ? ( reals[2] + 2.0 * M_PI ) : reals[2];
theta = ( theta > 2.0 * M_PI ) ? ( theta - 2.0 * M_PI ) : theta;
x = reals[0];
y = reals[1];
// Check for collision
unsigned int mx, my;
costmap->worldToMap( x, y, mx, my );
if ( static_cast<float>( costmap->getCost( mx, my ) ) >= INSCRIBED ) {
expansion.in_collision = true;
}
// Integrate path length
expansion.expansion_path_length += hypot( x - x_m, y - y_m );
x_m = x;
y_m = y;
// Store point
expansion.pts.emplace_back( x, y, theta );
}
}
template<typename IteratorT>
384 BoundaryExpansions Smoother::generateBoundaryExpansionPoints( IteratorT start, IteratorT end )
{
std::vector<double> distances = {
min_turning_rad_, // Radius
2.0 * min_turning_rad_, // Diameter
M_PI * min_turning_rad_, // 50% Circumference
2.0 * M_PI * min_turning_rad_ // Circumference
};
BoundaryExpansions boundary_expansions;
boundary_expansions.resize( distances.size( ) );
double curr_dist = 0.0;
double x_last = start->pose.position.x;
double y_last = start->pose.position.y;
geometry_msgs::msg::Point pt;
unsigned int curr_dist_idx = 0;
for ( IteratorT iter = start; iter != end; iter++ ) {
pt = iter->pose.position;
curr_dist += hypot( pt.x - x_last, pt.y - y_last );
x_last = pt.x;
y_last = pt.y;
if ( curr_dist >= distances[curr_dist_idx] ) {
boundary_expansions[curr_dist_idx].path_end_idx = iter - start;
boundary_expansions[curr_dist_idx].original_path_length = curr_dist;
curr_dist_idx++;
}
if ( curr_dist_idx == boundary_expansions.size( ) ) {
break;
}
}
return boundary_expansions;
}
421 void Smoother::enforceStartBoundaryConditions(
422 const geometry_msgs::msg::Pose & start_pose,
423 nav_msgs::msg::Path & path,
424 const nav2_costmap_2d::Costmap2D * costmap,
425 const bool & reversing_segment )
{
// Find range of points for testing
BoundaryExpansions boundary_expansions =
generateBoundaryExpansionPoints<PathIterator>( path.poses.begin( ), path.poses.end( ) );
// Generate the motion model and metadata from start -> test points
for ( unsigned int i = 0; i != boundary_expansions.size( ); i++ ) {
BoundaryExpansion & expansion = boundary_expansions[i];
if ( expansion.path_end_idx == 0.0 ) {
continue;
}
if ( !reversing_segment ) {
findBoundaryExpansion(
start_pose, path.poses[expansion.path_end_idx].pose, expansion,
costmap );
} else {
findBoundaryExpansion(
path.poses[expansion.path_end_idx].pose, start_pose, expansion,
costmap );
}
}
// Find the shortest kinematically feasible boundary expansion
unsigned int best_expansion_idx = findShortestBoundaryExpansionIdx( boundary_expansions );
if ( best_expansion_idx > boundary_expansions.size( ) ) {
return;
}
// Override values to match curve
BoundaryExpansion & best_expansion = boundary_expansions[best_expansion_idx];
if ( reversing_segment ) {
std::reverse( best_expansion.pts.begin( ), best_expansion.pts.end( ) );
}
for ( unsigned int i = 0; i != best_expansion.pts.size( ); i++ ) {
path.poses[i].pose.position.x = best_expansion.pts[i].x;
path.poses[i].pose.position.y = best_expansion.pts[i].y;
path.poses[i].pose.orientation = orientationAroundZAxis( best_expansion.pts[i].theta );
}
}
467 void Smoother::enforceEndBoundaryConditions(
468 const geometry_msgs::msg::Pose & end_pose,
469 nav_msgs::msg::Path & path,
470 const nav2_costmap_2d::Costmap2D * costmap,
471 const bool & reversing_segment )
{
// Find range of points for testing
BoundaryExpansions boundary_expansions =
generateBoundaryExpansionPoints<ReversePathIterator>( path.poses.rbegin( ), path.poses.rend( ) );
// Generate the motion model and metadata from start -> test points
unsigned int expansion_starting_idx;
for ( unsigned int i = 0; i != boundary_expansions.size( ); i++ ) {
BoundaryExpansion & expansion = boundary_expansions[i];
if ( expansion.path_end_idx == 0.0 ) {
continue;
}
expansion_starting_idx = path.poses.size( ) - expansion.path_end_idx - 1;
if ( !reversing_segment ) {
findBoundaryExpansion( path.poses[expansion_starting_idx].pose, end_pose, expansion, costmap );
} else {
findBoundaryExpansion( end_pose, path.poses[expansion_starting_idx].pose, expansion, costmap );
}
}
// Find the shortest kinematically feasible boundary expansion
unsigned int best_expansion_idx = findShortestBoundaryExpansionIdx( boundary_expansions );
if ( best_expansion_idx > boundary_expansions.size( ) ) {
return;
}
// Override values to match curve
BoundaryExpansion & best_expansion = boundary_expansions[best_expansion_idx];
if ( reversing_segment ) {
std::reverse( best_expansion.pts.begin( ), best_expansion.pts.end( ) );
}
expansion_starting_idx = path.poses.size( ) - best_expansion.path_end_idx - 1;
for ( unsigned int i = 0; i != best_expansion.pts.size( ); i++ ) {
path.poses[expansion_starting_idx + i].pose.position.x = best_expansion.pts[i].x;
path.poses[expansion_starting_idx + i].pose.position.y = best_expansion.pts[i].y;
path.poses[expansion_starting_idx + i].pose.orientation = orientationAroundZAxis(
best_expansion.pts[i].theta );
}
}
} // namespace nav2_smac_planner
1 // Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef DEPRECATED__SMOOTHER_HPP_
#define DEPRECATED__SMOOTHER_HPP_
#include <cmath>
#include <vector>
#include <iostream>
#include <memory>
#include <queue>
#include <utility>
#include "nav2_smac_planner/types.hpp"
#include "nav2_smac_planner/smoother_cost_function.hpp"
#include "ceres/ceres.h"
#include "Eigen/Core"
namespace nav2_smac_planner
{
/**
* @class nav2_smac_planner::Smoother
* @brief A Conjugate Gradient 2D path smoother implementation
*/
38 class Smoother
{
public:
/**
* @brief A constructor for nav2_smac_planner::Smoother
*/
44 Smoother( ) {}
/**
* @brief A destructor for nav2_smac_planner::Smoother
*/
49 ~Smoother( ) {}
/**
* @brief Initialization of the smoother
* @param params OptimizerParam struct
*/
55 void initialize( const OptimizerParams params )
{
_debug = params.debug;
// General Params
// 2 most valid options: STEEPEST_DESCENT, NONLINEAR_CONJUGATE_GRADIENT
_options.line_search_direction_type = ceres::NONLINEAR_CONJUGATE_GRADIENT;
_options.line_search_type = ceres::WOLFE;
_options.nonlinear_conjugate_gradient_type = ceres::POLAK_RIBIERE;
_options.line_search_interpolation_type = ceres::CUBIC;
_options.max_num_iterations = params.max_iterations;
_options.max_solver_time_in_seconds = params.max_time;
_options.function_tolerance = params.fn_tol;
_options.gradient_tolerance = params.gradient_tol;
_options.parameter_tolerance = params.param_tol;
_options.min_line_search_step_size = params.advanced.min_line_search_step_size;
_options.max_num_line_search_step_size_iterations =
params.advanced.max_num_line_search_step_size_iterations;
_options.line_search_sufficient_function_decrease =
params.advanced.line_search_sufficient_function_decrease;
_options.max_line_search_step_contraction = params.advanced.max_line_search_step_contraction;
_options.min_line_search_step_contraction = params.advanced.min_line_search_step_contraction;
_options.max_num_line_search_direction_restarts =
params.advanced.max_num_line_search_direction_restarts;
_options.line_search_sufficient_curvature_decrease =
params.advanced.line_search_sufficient_curvature_decrease;
_options.max_line_search_step_expansion = params.advanced.max_line_search_step_expansion;
if ( _debug ) {
_options.minimizer_progress_to_stdout = true;
} else {
_options.logging_type = ceres::SILENT;
}
}
/**
* @brief Smoother method
* @param path Reference to path
* @param costmap Pointer to minimal costmap
* @param smoother parameters weights
* @return If smoothing was successful
*/
101 bool smooth(
102 std::vector<Eigen::Vector2d> & path,
103 nav2_costmap_2d::Costmap2D * costmap,
const SmootherParams & params )
{
_options.max_solver_time_in_seconds = params.max_time;
#ifdef _MSC_VER
std::vector<double> parameters_vec( path.size( ) * 2 );
double * parameters = parameters_vec.data( );
#else
double parameters[path.size( ) * 2]; // NOLINT
#endif
for ( unsigned int i = 0; i != path.size( ); i++ ) {
parameters[2 * i] = path[i][0];
parameters[2 * i + 1] = path[i][1];
}
ceres::GradientProblemSolver::Summary summary;
ceres::GradientProblem problem( new UnconstrainedSmootherCostFunction( &path, costmap, params ) );
ceres::Solve( _options, problem, parameters, &summary );
if ( _debug ) {
std::cout << summary.FullReport( ) << '\n';
}
if ( !summary.IsSolutionUsable( ) || summary.initial_cost - summary.final_cost <= 0.0 ) {
return false;
}
for ( unsigned int i = 0; i != path.size( ); i++ ) {
path[i][0] = parameters[2 * i];
path[i][1] = parameters[2 * i + 1];
}
return true;
}
private:
140 bool _debug;
141 ceres::GradientProblemSolver::Options _options;
};
} // namespace nav2_smac_planner
#endif // DEPRECATED__SMOOTHER_HPP_
// Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef DEPRECATED__SMOOTHER_COST_FUNCTION_HPP_
#define DEPRECATED__SMOOTHER_COST_FUNCTION_HPP_
#include <cmath>
#include <vector>
#include <iostream>
#include <unordered_map>
#include <memory>
#include <queue>
#include <utility>
#include "ceres/ceres.h"
#include "Eigen/Core"
#include "nav2_smac_planner/types.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_smac_planner/options.hpp"
#define EPSILON 0.0001
namespace nav2_smac_planner
{
/**
* @struct nav2_smac_planner::UnconstrainedSmootherCostFunction
* @brief Cost function for path smoothing with multiple terms
* including curvature, smoothness, collision, and avoid obstacles.
*/
42 class UnconstrainedSmootherCostFunction : public ceres::FirstOrderFunction
{
public:
/**
* @brief A constructor for nav2_smac_planner::UnconstrainedSmootherCostFunction
* @param original_path Original unsmoothed path to smooth
* @param costmap A costmap to get values for collision and obstacle avoidance
*/
50 UnconstrainedSmootherCostFunction(
51 std::vector<Eigen::Vector2d> * original_path,
52 nav2_costmap_2d::Costmap2D * costmap,
const SmootherParams & params )
: _original_path( original_path ),
_num_params( 2 * original_path->size( ) ),
_costmap( costmap ),
_params( params )
{
// int height = costmap->getSizeInCellsX( );
// int width = costmap->getSizeInCellsY( );
// bool** binMap;
// binMap = new bool*[width];
// for ( int x = 0; x < width; x++ ) { binMap[x] = new bool[height]; }
// for ( int x = 0; x < width; ++x ) {
// for ( int y = 0; y < height; ++y ) {
// binMap[x][y] = costmap->getCost( x, y ) >= 253 ? true : false;
// }
// }
// voronoiDiagram.initializeMap( width, height, binMap );
// voronoiDiagram.update( );
// voronoiDiagram.visualize( );
}
/**
* @struct CurvatureComputations
* @brief Cache common computations between the curvature terms to minimize recomputations
*/
struct CurvatureComputations
{
/**
* @brief A constructor for nav2_smac_planner::CurvatureComputations
*/
CurvatureComputations( )
{
valid = true;
}
bool valid;
/**
* @brief Check if result is valid for penalty
* @return is valid ( non-nan, non-inf, and turning angle > max )
*/
96 bool isValid( )
{
return valid;
}
Eigen::Vector2d delta_xi{0.0, 0.0};
Eigen::Vector2d delta_xi_p{0.0, 0.0};
double delta_xi_norm{0};
double delta_xi_p_norm{0};
double delta_phi_i{0};
double turning_rad{0};
double ki_minus_kmax{0};
};
/**
* @brief Smoother cost function evaluation
* @param parameters X, Y pairs of points
* @param cost total cost of path
* @param gradient of path at each X, Y pair from cost function derived analytically
* @return if successful in computing values
*/
virtual bool Evaluate(
const double * parameters,
double * cost,
double * gradient ) const
{
Eigen::Vector2d xi;
Eigen::Vector2d xi_p1;
Eigen::Vector2d xi_m1;
unsigned int x_index, y_index;
cost[0] = 0.0;
double cost_raw = 0.0;
double grad_x_raw = 0.0;
double grad_y_raw = 0.0;
unsigned int mx, my;
bool valid_coords = true;
double costmap_cost = 0.0;
// cache some computations between the residual and jacobian
CurvatureComputations curvature_params;
for ( int i = 0; i != NumParameters( ) / 2; i++ ) {
x_index = 2 * i;
y_index = 2 * i + 1;
gradient[x_index] = 0.0;
gradient[y_index] = 0.0;
if ( i < 1 || i >= NumParameters( ) / 2 - 1 ) {
continue;
}
xi = Eigen::Vector2d( parameters[x_index], parameters[y_index] );
xi_p1 = Eigen::Vector2d( parameters[x_index + 2], parameters[y_index + 2] );
xi_m1 = Eigen::Vector2d( parameters[x_index - 2], parameters[y_index - 2] );
// compute cost
addSmoothingResidual( _params.smooth_weight, xi, xi_p1, xi_m1, cost_raw );
addCurvatureResidual( _params.curvature_weight, xi, xi_p1, xi_m1, curvature_params, cost_raw );
addDistanceResidual( _params.distance_weight, xi, _original_path->at( i ), cost_raw );
if ( valid_coords = _costmap->worldToMap( xi[0], xi[1], mx, my ) ) {
costmap_cost = _costmap->getCost( mx, my );
addCostResidual( _params.costmap_weight, costmap_cost, cost_raw, xi );
}
if ( gradient != NULL ) {
// compute gradient
gradient[x_index] = 0.0;
gradient[y_index] = 0.0;
addSmoothingJacobian( _params.smooth_weight, xi, xi_p1, xi_m1, grad_x_raw, grad_y_raw );
addCurvatureJacobian(
_params.curvature_weight, xi, xi_p1, xi_m1, curvature_params,
grad_x_raw, grad_y_raw );
addDistanceJacobian(
_params.distance_weight, xi, _original_path->at(
i ), grad_x_raw, grad_y_raw );
if ( valid_coords ) {
addCostJacobian( _params.costmap_weight, mx, my, costmap_cost, grad_x_raw, grad_y_raw );
}
gradient[x_index] = grad_x_raw;
gradient[y_index] = grad_y_raw;
}
}
cost[0] = cost_raw;
return true;
}
/**
* @brief Get number of parameter blocks
* @return Number of parameters in cost function
*/
virtual int NumParameters( ) const {return _num_params;}
protected:
/**
* @brief Cost function term for smooth paths
* @param weight Weight to apply to function
* @param pt Point Xi for evaluation
* @param pt Point Xi+1 for calculating Xi's cost
* @param pt Point Xi-1 for calculating Xi's cost
* @param r Residual ( cost ) of term
*/
inline void addSmoothingResidual(
const double & weight,
const Eigen::Vector2d & pt,
const Eigen::Vector2d & pt_p,
const Eigen::Vector2d & pt_m,
double & r ) const
{
r += weight * (
pt_p.dot( pt_p ) -
4 * pt_p.dot( pt ) +
2 * pt_p.dot( pt_m ) +
4 * pt.dot( pt ) -
4 * pt.dot( pt_m ) +
pt_m.dot( pt_m ) ); // objective function value
}
/**
* @brief Cost function derivative term for smooth paths
* @param weight Weight to apply to function
* @param pt Point Xi for evaluation
* @param pt Point Xi+1 for calculating Xi's cost
* @param pt Point Xi-1 for calculating Xi's cost
* @param j0 Gradient of X term
* @param j1 Gradient of Y term
*/
inline void addSmoothingJacobian(
const double & weight,
const Eigen::Vector2d & pt,
const Eigen::Vector2d & pt_p,
const Eigen::Vector2d & pt_m,
double & j0,
double & j1 ) const
{
j0 += weight *
( -4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0] ); // xi x component of partial-derivative
j1 += weight *
( -4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1] ); // xi y component of partial-derivative
}
/**
* @brief Cost function term for maximum curved paths
* @param weight Weight to apply to function
* @param pt Point Xi for evaluation
* @param pt Point Xi+1 for calculating Xi's cost
* @param pt Point Xi-1 for calculating Xi's cost
* @param curvature_params A struct to cache computations for the jacobian to use
* @param r Residual ( cost ) of term
*/
inline void addCurvatureResidual(
const double & weight,
const Eigen::Vector2d & pt,
const Eigen::Vector2d & pt_p,
const Eigen::Vector2d & pt_m,
CurvatureComputations & curvature_params,
double & r ) const
{
curvature_params.valid = true;
curvature_params.delta_xi = Eigen::Vector2d( pt[0] - pt_m[0], pt[1] - pt_m[1] );
curvature_params.delta_xi_p = Eigen::Vector2d( pt_p[0] - pt[0], pt_p[1] - pt[1] );
curvature_params.delta_xi_norm = curvature_params.delta_xi.norm( );
curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm( );
if ( curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON ||
std::isnan( curvature_params.delta_xi_p_norm ) || std::isnan( curvature_params.delta_xi_norm ) ||
std::isinf( curvature_params.delta_xi_p_norm ) || std::isinf( curvature_params.delta_xi_norm ) )
{
// ensure we have non-nan values returned
curvature_params.valid = false;
return;
}
const double & delta_xi_by_xi_p =
curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm;
double projection =
curvature_params.delta_xi.dot( curvature_params.delta_xi_p ) / delta_xi_by_xi_p;
if ( fabs( 1 - projection ) < EPSILON || fabs( projection + 1 ) < EPSILON ) {
projection = 1.0;
}
curvature_params.delta_phi_i = std::acos( projection );
curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm;
curvature_params.ki_minus_kmax = curvature_params.turning_rad - _params.max_curvature;
if ( curvature_params.ki_minus_kmax <= EPSILON ) {
// Quadratic penalty need not apply
curvature_params.valid = false;
return;
}
r += weight *
curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax; // objective function value
}
/**
* @brief Cost function derivative term for maximum curvature paths
* @param weight Weight to apply to function
* @param pt Point Xi for evaluation
* @param pt Point Xi+1 for calculating Xi's cost
* @param pt Point Xi-1 for calculating Xi's cost
* @param curvature_params A struct with cached values to speed up Jacobian computation
* @param j0 Gradient of X term
* @param j1 Gradient of Y term
*/
inline void addCurvatureJacobian(
const double & weight,
const Eigen::Vector2d & pt,
const Eigen::Vector2d & pt_p,
const Eigen::Vector2d & /*pt_m*/,
CurvatureComputations & curvature_params,
double & j0,
double & j1 ) const
{
if ( !curvature_params.isValid( ) ) {
return;
}
const double & partial_delta_phi_i_wrt_cost_delta_phi_i =
-1 / std::sqrt( 1 - std::pow( std::cos( curvature_params.delta_phi_i ), 2 ) );
// const Eigen::Vector2d ones = Eigen::Vector2d( 1.0, 1.0 );
auto neg_pt_plus = -1 * pt_p;
Eigen::Vector2d p1 = normalizedOrthogonalComplement(
pt, neg_pt_plus, curvature_params.delta_xi_norm, curvature_params.delta_xi_p_norm );
Eigen::Vector2d p2 = normalizedOrthogonalComplement(
neg_pt_plus, pt, curvature_params.delta_xi_p_norm, curvature_params.delta_xi_norm );
const double & u = 2 * curvature_params.ki_minus_kmax;
const double & common_prefix =
( 1 / curvature_params.delta_xi_norm ) * partial_delta_phi_i_wrt_cost_delta_phi_i;
const double & common_suffix = curvature_params.delta_phi_i /
( curvature_params.delta_xi_norm * curvature_params.delta_xi_norm );
const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi /
curvature_params.delta_xi_norm;
const Eigen::Vector2d jacobian = u *
( common_prefix * ( -p1 - p2 ) - ( common_suffix * d_delta_xi_d_xi ) );
const Eigen::Vector2d jacobian_im1 = u *
( common_prefix * p2 + ( common_suffix * d_delta_xi_d_xi ) );
const Eigen::Vector2d jacobian_ip1 = u * ( common_prefix * p1 );
// Old formulation we may require again.
// j0 += weight *
// ( jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0] );
// j1 += weight *
// ( jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1] );
j0 += weight * jacobian[0]; // xi x component of partial-derivative
j1 += weight * jacobian[1]; // xi x component of partial-derivative
}
/**
* @brief Cost function derivative term for steering away changes in pose
* @param weight Weight to apply to function
* @param xi Point Xi for evaluation
* @param xi_original original point Xi for evaluation
* @param r Residual ( cost ) of term
*/
inline void addDistanceResidual(
const double & weight,
const Eigen::Vector2d & xi,
const Eigen::Vector2d & xi_original,
double & r ) const
{
r += weight * ( xi - xi_original ).dot( xi - xi_original ); // objective function value
}
/**
* @brief Cost function derivative term for steering away changes in pose
* @param weight Weight to apply to function
* @param xi Point Xi for evaluation
* @param xi_original original point Xi for evaluation
* @param j0 Gradient of X term
* @param j1 Gradient of Y term
*/
inline void addDistanceJacobian(
const double & weight,
const Eigen::Vector2d & xi,
const Eigen::Vector2d & xi_original,
double & j0,
double & j1 ) const
{
j0 += weight * 2 * ( xi[0] - xi_original[0] ); // xi y component of partial-derivative
j1 += weight * 2 * ( xi[1] - xi_original[1] ); // xi y component of partial-derivative
}
/**
* @brief Cost function term for steering away from costs
* @param weight Weight to apply to function
* @param value Point Xi's cost'
* @param params computed values to reduce overhead
* @param r Residual ( cost ) of term
*/
inline void addCostResidual(
const double & weight,
const double & value,
double & r,
Eigen::Vector2d & xi ) const
{
if ( value == FREE ) {
return;
}
r += weight * value * value; // objective function value
// float obsDst = voronoiDiagram.getDistance( ( int )xi[0], ( int )xi[1] );
// if ( abs( obsDst ) > 0.3 ) {
// return;
// }
// r += weight * ( abs( obsDst ) - 0.3 ) * ( abs( obsDst ) - 0.3 );
}
/**
* @brief Cost function derivative term for steering away from costs
* @param weight Weight to apply to function
* @param mx Point Xi's x coordinate in map frame
* @param mx Point Xi's y coordinate in map frame
* @param value Point Xi's cost'
* @param params computed values to reduce overhead
* @param j0 Gradient of X term
* @param j1 Gradient of Y term
*/
inline void addCostJacobian(
const double & weight,
const unsigned int & mx,
const unsigned int & my,
const double & value,
double & j0,
double & j1 ) const
{
if ( value == FREE ) {
return;
}
const Eigen::Vector2d grad = getCostmapGradient( mx, my );
const double common_prefix = -2.0 * _params.costmap_factor * weight * value * value;
j0 += common_prefix * grad[0]; // xi x component of partial-derivative
j1 += common_prefix * grad[1]; // xi y component of partial-derivative
}
/**
* @brief Computing the gradient of the costmap using
* the 2 point numerical differentiation method
* @param mx Point Xi's x coordinate in map frame
* @param mx Point Xi's y coordinate in map frame
* @param params Params reference to store gradients
*/
inline Eigen::Vector2d getCostmapGradient(
const unsigned int mx,
const unsigned int my ) const
{
// find unit vector that describes that direction
// via 7 point taylor series approximation for gradient at Xi
Eigen::Vector2d gradient;
double l_1 = 0.0;
double l_2 = 0.0;
double l_3 = 0.0;
double r_1 = 0.0;
double r_2 = 0.0;
double r_3 = 0.0;
if ( mx < _costmap->getSizeInCellsX( ) ) {
r_1 = static_cast<double>( _costmap->getCost( mx + 1, my ) );
}
if ( mx + 1 < _costmap->getSizeInCellsX( ) ) {
r_2 = static_cast<double>( _costmap->getCost( mx + 2, my ) );
}
if ( mx + 2 < _costmap->getSizeInCellsX( ) ) {
r_3 = static_cast<double>( _costmap->getCost( mx + 3, my ) );
}
if ( mx > 0 ) {
l_1 = static_cast<double>( _costmap->getCost( mx - 1, my ) );
}
if ( mx - 1 > 0 ) {
l_2 = static_cast<double>( _costmap->getCost( mx - 2, my ) );
}
if ( mx - 2 > 0 ) {
l_3 = static_cast<double>( _costmap->getCost( mx - 3, my ) );
}
gradient[1] = ( 45 * r_1 - 9 * r_2 + r_3 - 45 * l_1 + 9 * l_2 - l_3 ) / 60;
if ( my < _costmap->getSizeInCellsY( ) ) {
r_1 = static_cast<double>( _costmap->getCost( mx, my + 1 ) );
}
if ( my + 1 < _costmap->getSizeInCellsY( ) ) {
r_2 = static_cast<double>( _costmap->getCost( mx, my + 2 ) );
}
if ( my + 2 < _costmap->getSizeInCellsY( ) ) {
r_3 = static_cast<double>( _costmap->getCost( mx, my + 3 ) );
}
if ( my > 0 ) {
l_1 = static_cast<double>( _costmap->getCost( mx, my - 1 ) );
}
if ( my - 1 > 0 ) {
l_2 = static_cast<double>( _costmap->getCost( mx, my - 2 ) );
}
if ( my - 2 > 0 ) {
l_3 = static_cast<double>( _costmap->getCost( mx, my - 3 ) );
}
gradient[0] = ( 45 * r_1 - 9 * r_2 + r_3 - 45 * l_1 + 9 * l_2 - l_3 ) / 60;
gradient.normalize( );
return gradient;
}
/**
* @brief Computing the normalized orthogonal component of 2 vectors
* @param a Vector
* @param b Vector
* @param norm a Vector's norm
* @param norm b Vector's norm
* @return Normalized vector of orthogonal components
*/
inline Eigen::Vector2d normalizedOrthogonalComplement(
const Eigen::Vector2d & a,
const Eigen::Vector2d & b,
const double & a_norm,
const double & b_norm ) const
{
return ( a - ( a.dot( b ) * b / b.squaredNorm( ) ) ) / ( a_norm * b_norm );
}
std::vector<Eigen::Vector2d> * _original_path{nullptr};
int _num_params;
nav2_costmap_2d::Costmap2D * _costmap{nullptr};
SmootherParams _params;
// DynamicVoronoi voronoiDiagram;
};
} // namespace nav2_smac_planner
#endif // DEPRECATED__SMOOTHER_COST_FUNCTION_HPP_
1 // Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef DEPRECATED__UPSAMPLER_HPP_
#define DEPRECATED__UPSAMPLER_HPP_
#include <cmath>
#include <vector>
#include <iostream>
#include <memory>
#include <queue>
#include <algorithm>
#include <utility>
#include "nav2_smac_planner/types.hpp"
#include "nav2_smac_planner/upsampler_cost_function.hpp"
#include "nav2_smac_planner/upsampler_cost_function_nlls.hpp"
#include "ceres/ceres.h"
#include "Eigen/Core"
namespace nav2_smac_planner
{
/**
* @class nav2_smac_planner::Upsampler
* @brief A Conjugate Gradient 2D path upsampler implementation
*/
40 class Upsampler
{
public:
/**
* @brief A constructor for nav2_smac_planner::Upsampler
*/
46 Upsampler( ) {}
/**
* @brief A destructor for nav2_smac_planner::Upsampler
*/
51 ~Upsampler( ) {}
/**
* @brief Initialization of the Upsampler
*/
56 void initialize( const OptimizerParams params )
{
_debug = params.debug;
// General Params
// 2 most valid options: STEEPEST_DESCENT, NONLINEAR_CONJUGATE_GRADIENT
_options.line_search_direction_type = ceres::NONLINEAR_CONJUGATE_GRADIENT;
_options.line_search_type = ceres::WOLFE;
_options.nonlinear_conjugate_gradient_type = ceres::POLAK_RIBIERE;
_options.line_search_interpolation_type = ceres::CUBIC;
_options.max_num_iterations = params.max_iterations; // 5000
_options.max_solver_time_in_seconds = params.max_time; // 5.0; // TODO
_options.function_tolerance = params.fn_tol;
_options.gradient_tolerance = params.gradient_tol;
_options.parameter_tolerance = params.param_tol; // 1e-20;
_options.min_line_search_step_size = params.advanced.min_line_search_step_size; // 1e-30;
_options.max_num_line_search_step_size_iterations =
params.advanced.max_num_line_search_step_size_iterations;
_options.line_search_sufficient_function_decrease =
params.advanced.line_search_sufficient_function_decrease; // 1e-30;
_options.max_line_search_step_contraction = params.advanced.max_line_search_step_contraction;
_options.min_line_search_step_contraction = params.advanced.min_line_search_step_contraction;
_options.max_num_line_search_direction_restarts =
params.advanced.max_num_line_search_direction_restarts;
_options.line_search_sufficient_curvature_decrease =
params.advanced.line_search_sufficient_curvature_decrease;
_options.max_line_search_step_expansion = params.advanced.max_line_search_step_expansion;
if ( _debug ) {
_options.minimizer_progress_to_stdout = true;
} else {
_options.logging_type = ceres::SILENT;
}
}
/**
* @brief Upsampling method
* @param path Reference to path
* @param upsample parameters weights
* @param upsample_ratio upsample ratio
* @return If Upsampler was successful
*/
102 bool upsample(
103 std::vector<Eigen::Vector2d> & path,
const SmootherParams & params,
const int & upsample_ratio )
{
_options.max_solver_time_in_seconds = params.max_time;
if ( upsample_ratio != 2 && upsample_ratio != 4 ) {
// invalid inputs
return false;
}
const int param_ratio = upsample_ratio * 2.0;
const int total_size = 2 * ( path.size( ) * upsample_ratio - upsample_ratio + 1 );
double parameters[total_size]; // NOLINT
// 20-4hz regularly, but dosnt work in faster cases
// Linearly distribute initial poses for optimization
// TODO( stevemacenski ) generalize for 2x and 4x
unsigned int next_pt;
Eigen::Vector2d interpolated;
std::vector<Eigen::Vector2d> temp_path;
for ( unsigned int pt = 0; pt != path.size( ) - 1; pt++ ) {
next_pt = pt + 1;
interpolated = ( path[next_pt] + path[pt] ) / 2.0;
parameters[param_ratio * pt] = path[pt][0];
parameters[param_ratio * pt + 1] = path[pt][1];
temp_path.push_back( path[pt] );
parameters[param_ratio * pt + 2] = interpolated[0];
parameters[param_ratio * pt + 3] = interpolated[1];
temp_path.push_back( interpolated );
}
parameters[total_size - 2] = path.back( )[0];
parameters[total_size - 1] = path.back( )[1];
temp_path.push_back( path.back( ) );
// Solve the upsampling problem
ceres::GradientProblemSolver::Summary summary;
ceres::GradientProblem problem( new UpsamplerCostFunction( temp_path, params, upsample_ratio ) );
ceres::Solve( _options, problem, parameters, &summary );
path.resize( total_size / 2 );
for ( int i = 0; i != total_size / 2; i++ ) {
path[i][0] = parameters[2 * i];
path[i][1] = parameters[2 * i + 1];
}
// 10-15 hz, regularly
// std::vector<Eigen::Vector2d> path_double_sampled;
// for ( int i = 0; i != path.size( ) - 1; i++ ) { // last term should not be upsampled
// path_double_sampled.push_back( path[i] );
// path_double_sampled.push_back( ( path[i+1] + path[i] ) / 2 );
// }
// std::unique_ptr<ceres::Problem> problem = std::make_unique<ceres::Problem>( );
// for ( uint i = 1; i != path_double_sampled.size( ) - 1; i++ ) {
// ceres::CostFunction * cost_fn =
// new UpsamplerConstrainedCostFunction( path_double_sampled, params, 2, i );
// problem->AddResidualBlock(
// cost_fn, nullptr, &path_double_sampled[i][0], &path_double_sampled[i][1] );
// // locking initial coordinates unnecessary since there's no update between terms in NLLS
// }
// ceres::Solver::Summary summary;
// _options.minimizer_type = ceres::LINE_SEARCH;
// ceres::Solve( _options, problem.get( ), &summary );
// if ( upsample_ratio == 4 ) {
// std::vector<Eigen::Vector2d> path_quad_sampled;
// for ( int i = 0; i != path_double_sampled.size( ) - 1; i++ ) {
// path_quad_sampled.push_back( path_double_sampled[i] );
// path_quad_sampled.push_back( ( path_double_sampled[i+1] + path_double_sampled[i] ) / 2.0 );
// }
// std::unique_ptr<ceres::Problem> problem2 = std::make_unique<ceres::Problem>( );
// for ( uint i = 1; i != path_quad_sampled.size( ) - 1; i++ ) {
// ceres::CostFunction * cost_fn =
// new UpsamplerConstrainedCostFunction( path_quad_sampled, params, 4, i );
// problem2->AddResidualBlock(
// cost_fn, nullptr, &path_quad_sampled[i][0], &path_quad_sampled[i][1] );
// }
// ceres::Solve( _options, problem2.get( ), &summary );
// path = path_quad_sampled;
// } else {
// path = path_double_sampled;
// }
if ( _debug ) {
std::cout << summary.FullReport( ) << '\n';
}
if ( !summary.IsSolutionUsable( ) || summary.initial_cost - summary.final_cost <= 0.0 ) {
return false;
}
return true;
}
private:
207 bool _debug;
208 ceres::GradientProblemSolver::Options _options;
};
} // namespace nav2_smac_planner
#endif // DEPRECATED__UPSAMPLER_HPP_
// Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef DEPRECATED__UPSAMPLER_COST_FUNCTION_HPP_
#define DEPRECATED__UPSAMPLER_COST_FUNCTION_HPP_
#include <cmath>
#include <vector>
#include <iostream>
#include <unordered_map>
#include <memory>
#include <queue>
#include <utility>
#include "ceres/ceres.h"
#include "Eigen/Core"
#include "nav2_smac_planner/types.hpp"
#include "nav2_smac_planner/options.hpp"
#define EPSILON 0.0001
namespace nav2_smac_planner
{
/**
* @struct nav2_smac_planner::UpsamplerCostFunction
* @brief Cost function for path upsampling with multiple terms using unconstrained
* optimization including curvature, smoothness, collision, and avoid obstacles.
*/
40 class UpsamplerCostFunction : public ceres::FirstOrderFunction
{
public:
/**
* @brief A constructor for nav2_smac_planner::UpsamplerCostFunction
* @param num_points Number of path points to consider
*/
47 UpsamplerCostFunction(
48 const std::vector<Eigen::Vector2d> & path,
const SmootherParams & params,
const int & upsample_ratio )
: _num_params( 2 * path.size( ) ),
_params( params ),
_upsample_ratio( upsample_ratio ),
_path( path )
{
}
// TODO( stevemacenski ) removed upsample_ratio because temp upsampling on path size
/**
* @struct CurvatureComputations
* @brief Cache common computations between the curvature terms to minimize recomputations
*/
struct CurvatureComputations
{
/**
* @brief A constructor for nav2_smac_planner::CurvatureComputations
*/
CurvatureComputations( )
{
valid = false;
}
bool valid;
/**
* @brief Check if result is valid for penalty
* @return is valid ( non-nan, non-inf, and turning angle > max )
*/
78 bool isValid( )
{
return valid;
}
Eigen::Vector2d delta_xi{0, 0};
Eigen::Vector2d delta_xi_p{0, 0};
double delta_xi_norm{0};
double delta_xi_p_norm{0};
double delta_phi_i{0};
double turning_rad{0};
double ki_minus_kmax{0};
};
/**
* @brief Smoother cost function evaluation
* @param parameters X, Y pairs of points
* @param cost total cost of path
* @param gradient of path at each X, Y pair from cost function derived analytically
* @return if successful in computing values
*/
virtual bool Evaluate(
const double * parameters,
double * cost,
double * gradient ) const
{
Eigen::Vector2d xi;
Eigen::Vector2d xi_p1;
Eigen::Vector2d xi_m1;
uint x_index, y_index;
cost[0] = 0.0;
double cost_raw = 0.0;
double grad_x_raw = 0.0;
double grad_y_raw = 0.0;
// cache some computations between the residual and jacobian
CurvatureComputations curvature_params;
for ( int i = 0; i != NumParameters( ) / 2; i++ ) {
x_index = 2 * i;
y_index = 2 * i + 1;
gradient[x_index] = 0.0;
gradient[y_index] = 0.0;
if ( i < 1 || i >= NumParameters( ) / 2 - 1 ) {
continue;
}
// if original point's neighbors TODO
if ( i % _upsample_ratio == 1 ) {
continue;
}
xi = Eigen::Vector2d( parameters[x_index], parameters[y_index] );
// TODO( stevemacenski ): from deep copy to make sure no feedback _path
xi_p1 = _path.at( i + 1 );
xi_m1 = _path.at( i - 1 );
// xi_p1 = Eigen::Vector2d( parameters[x_index + 2], parameters[y_index + 2] );
// xi_m1 = Eigen::Vector2d( parameters[x_index - 2], parameters[y_index - 2] );
// compute cost
addSmoothingResidual( 15000, xi, xi_p1, xi_m1, cost_raw );
addCurvatureResidual( 60.0, xi, xi_p1, xi_m1, curvature_params, cost_raw );
if ( gradient != NULL ) {
// compute gradient
addSmoothingJacobian( 15000, xi, xi_p1, xi_m1, grad_x_raw, grad_y_raw );
addCurvatureJacobian( 60.0, xi, xi_p1, xi_m1, curvature_params, grad_x_raw, grad_y_raw );
gradient[x_index] = grad_x_raw;
gradient[y_index] = grad_y_raw;
}
}
cost[0] = cost_raw;
return true;
}
/**
* @brief Get number of parameter blocks
* @return Number of parameters in cost function
*/
virtual int NumParameters( ) const {return _num_params;}
protected:
/**
* @brief Cost function term for smooth paths
* @param weight Weight to apply to function
* @param pt Point Xi for evaluation
* @param pt Point Xi+1 for calculating Xi's cost
* @param pt Point Xi-1 for calculating Xi's cost
* @param r Residual ( cost ) of term
*/
inline void addSmoothingResidual(
const double & weight,
const Eigen::Vector2d & pt,
const Eigen::Vector2d & pt_p,
const Eigen::Vector2d & pt_m,
double & r ) const
{
r += weight * (
pt_p.dot( pt_p ) -
4 * pt_p.dot( pt ) +
2 * pt_p.dot( pt_m ) +
4 * pt.dot( pt ) -
4 * pt.dot( pt_m ) +
pt_m.dot( pt_m ) ); // objective function value
}
/**
* @brief Cost function derivative term for smooth paths
* @param weight Weight to apply to function
* @param pt Point Xi for evaluation
* @param pt Point Xi+1 for calculating Xi's cost
* @param pt Point Xi-1 for calculating Xi's cost
* @param j0 Gradient of X term
* @param j1 Gradient of Y term
*/
inline void addSmoothingJacobian(
const double & weight,
const Eigen::Vector2d & pt,
const Eigen::Vector2d & pt_p,
const Eigen::Vector2d & pt_m,
double & j0,
double & j1 ) const
{
j0 += weight *
( -4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0] ); // xi x component of partial-derivative
j1 += weight *
( -4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1] ); // xi y component of partial-derivative
}
/**
* @brief Get path curvature information
* @param pt Point Xi for evaluation
* @param pt Point Xi+1 for calculating Xi's cost
* @param pt Point Xi-1 for calculating Xi's cost
* @param curvature_params A struct to cache computations for the jacobian to use
*/
inline void getCurvatureParams(
const Eigen::Vector2d & pt,
const Eigen::Vector2d & pt_p,
const Eigen::Vector2d & pt_m,
CurvatureComputations & curvature_params ) const
{
curvature_params.valid = true;
curvature_params.delta_xi = Eigen::Vector2d( pt[0] - pt_m[0], pt[1] - pt_m[1] );
curvature_params.delta_xi_p = Eigen::Vector2d( pt_p[0] - pt[0], pt_p[1] - pt[1] );
curvature_params.delta_xi_norm = curvature_params.delta_xi.norm( );
curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm( );
if ( curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON ||
std::isnan( curvature_params.delta_xi_p_norm ) || std::isnan( curvature_params.delta_xi_norm ) ||
std::isinf( curvature_params.delta_xi_p_norm ) || std::isinf( curvature_params.delta_xi_norm ) )
{
// ensure we have non-nan values returned
curvature_params.valid = false;
return;
}
const double & delta_xi_by_xi_p =
curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm;
double projection =
curvature_params.delta_xi.dot( curvature_params.delta_xi_p ) / delta_xi_by_xi_p;
if ( fabs( 1 - projection ) < EPSILON || fabs( projection + 1 ) < EPSILON ) {
projection = 1.0;
}
curvature_params.delta_phi_i = std::acos( projection );
curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm;
curvature_params.ki_minus_kmax = curvature_params.turning_rad - _upsample_ratio *
_params.max_curvature;
// TODO( stevemacenski ) is use of upsample_ratio correct here? small number?
// TODO( stevemacenski ) can remove the subtraction with a
// lower weight value, does have direction issue, maybe just tuning?
if ( curvature_params.ki_minus_kmax <= EPSILON ) {
// Quadratic penalty need not apply
curvature_params.valid = false;
}
}
/**
* @brief Cost function term for maximum curved paths
* @param weight Weight to apply to function
* @param pt Point Xi for evaluation
* @param pt Point Xi+1 for calculating Xi's cost
* @param pt Point Xi-1 for calculating Xi's cost
* @param curvature_params A struct to cache computations for the jacobian to use
* @param r Residual ( cost ) of term
*/
inline void addCurvatureResidual(
const double & weight,
const Eigen::Vector2d & pt,
const Eigen::Vector2d & pt_p,
const Eigen::Vector2d & pt_m,
CurvatureComputations & curvature_params,
double & r ) const
{
getCurvatureParams( pt, pt_p, pt_m, curvature_params );
if ( !curvature_params.isValid( ) ) {
return;
}
r += weight *
curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax; // objective function value
}
/**
* @brief Cost function derivative term for maximum curvature paths
* @param weight Weight to apply to function
* @param pt Point Xi for evaluation
* @param pt Point Xi+1 for calculating Xi's cost
* @param pt Point Xi-1 for calculating Xi's cost
* @param curvature_params A struct with cached values to speed up Jacobian computation
* @param j0 Gradient of X term
* @param j1 Gradient of Y term
*/
inline void addCurvatureJacobian(
const double & weight,
const Eigen::Vector2d & pt,
const Eigen::Vector2d & pt_p,
const Eigen::Vector2d & /*pt_m*/,
CurvatureComputations & curvature_params,
double & j0,
double & j1 ) const
{
if ( !curvature_params.isValid( ) ) {
return;
}
const double & partial_delta_phi_i_wrt_cost_delta_phi_i =
-1 / std::sqrt( 1 - std::pow( std::cos( curvature_params.delta_phi_i ), 2 ) );
// const Eigen::Vector2d ones = Eigen::Vector2d( 1.0, 1.0 );
auto neg_pt_plus = -1 * pt_p;
Eigen::Vector2d p1 = normalizedOrthogonalComplement(
pt, neg_pt_plus, curvature_params.delta_xi_norm, curvature_params.delta_xi_p_norm );
Eigen::Vector2d p2 = normalizedOrthogonalComplement(
neg_pt_plus, pt, curvature_params.delta_xi_p_norm, curvature_params.delta_xi_norm );
const double & u = 2 * curvature_params.ki_minus_kmax;
const double & common_prefix =
( 1 / curvature_params.delta_xi_norm ) * partial_delta_phi_i_wrt_cost_delta_phi_i;
const double & common_suffix = curvature_params.delta_phi_i /
( curvature_params.delta_xi_norm * curvature_params.delta_xi_norm );
const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi /
curvature_params.delta_xi_norm;
const Eigen::Vector2d jacobian = u *
( common_prefix * ( -p1 - p2 ) - ( common_suffix * d_delta_xi_d_xi ) );
const Eigen::Vector2d jacobian_im1 = u *
( common_prefix * p2 + ( common_suffix * d_delta_xi_d_xi ) );
const Eigen::Vector2d jacobian_ip1 = u * ( common_prefix * p1 );
j0 += weight * jacobian[0]; // xi y component of partial-derivative
j1 += weight * jacobian[1]; // xi x component of partial-derivative
// j0 += weight *
// ( jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0] );
// j1 += weight *
// ( jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1] );
}
/**
* @brief Computing the normalized orthogonal component of 2 vectors
* @param a Vector
* @param b Vector
* @param norm a Vector's norm
* @param norm b Vector's norm
* @return Normalized vector of orthogonal components
*/
inline Eigen::Vector2d normalizedOrthogonalComplement(
const Eigen::Vector2d & a,
const Eigen::Vector2d & b,
const double & a_norm,
const double & b_norm ) const
{
return ( a - ( a.dot( b ) * b / b.squaredNorm( ) ) ) / ( a_norm * b_norm );
}
int _num_params;
SmootherParams _params;
int _upsample_ratio;
std::vector<Eigen::Vector2d> _path;
};
} // namespace nav2_smac_planner
#endif // DEPRECATED__UPSAMPLER_COST_FUNCTION_HPP_
// Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef DEPRECATED__UPSAMPLER_COST_FUNCTION_NLLS_HPP_
#define DEPRECATED__UPSAMPLER_COST_FUNCTION_NLLS_HPP_
#include <cmath>
#include <vector>
#include <iostream>
#include <unordered_map>
#include <memory>
#include <queue>
#include <utility>
#include "ceres/ceres.h"
#include "Eigen/Core"
#include "nav2_smac_planner/types.hpp"
#include "nav2_smac_planner/options.hpp"
#define EPSILON 0.0001
namespace nav2_smac_planner
{
/**
* @struct nav2_smac_planner::UpsamplerConstrainedCostFunction
* @brief Cost function for path upsampling with multiple terms using NLLS
* including curvature, smoothness, collision, and avoid obstacles.
*/
40 class UpsamplerConstrainedCostFunction : public ceres::SizedCostFunction<1, 1, 1>
{
public:
/**
* @brief A constructor for nav2_smac_planner::UpsamplerConstrainedCostFunction
* @param num_points Number of path points to consider
*/
47 UpsamplerConstrainedCostFunction(
48 const std::vector<Eigen::Vector2d> & path,
const SmootherParams & params,
const int & upsample_ratio,
const int & i )
: _path( path ),
_params( params ),
_upsample_ratio( upsample_ratio ),
index( i )
{
}
/**
* @struct CurvatureComputations
* @brief Cache common computations between the curvature terms to minimize recomputations
*/
struct CurvatureComputations
{
/**
* @brief A constructor for nav2_smac_planner::CurvatureComputations
*/
CurvatureComputations( )
{
valid = true;
}
bool valid;
/**
* @brief Check if result is valid for penalty
* @return is valid ( non-nan, non-inf, and turning angle > max )
*/
78 bool isValid( )
{
return valid;
}
Eigen::Vector2d delta_xi{0, 0};
Eigen::Vector2d delta_xi_p{0, 0};
double delta_xi_norm{0};
double delta_xi_p_norm{0};
double delta_phi_i{0};
double turning_rad{0};
double ki_minus_kmax{0};
};
/**
* @brief Smoother cost function evaluation
* @param parameters X, Y pairs of points
* @param cost total cost of path
* @param gradient of path at each X, Y pair from cost function derived analytically
* @return if successful in computing values
*/
bool Evaluate(
double const * const * parameters,
double * residuals,
double ** jacobians ) const override
{
Eigen::Vector2d xi = Eigen::Vector2d( parameters[0][0], parameters[1][0] );
Eigen::Vector2d xi_p1 = _path.at( index + 1 );
Eigen::Vector2d xi_m1 = _path.at( index - 1 );
CurvatureComputations curvature_params;
double grad_x_raw = 0, grad_y_raw = 0, cost_raw = 0;
// compute cost
addSmoothingResidual( 15000, xi, xi_p1, xi_m1, cost_raw );
addCurvatureResidual( 60.0, xi, xi_p1, xi_m1, curvature_params, cost_raw );
residuals[0] = 0;
residuals[0] = cost_raw; // objective function value x
if ( jacobians != NULL && jacobians[0] != NULL ) {
addSmoothingJacobian( 15000, xi, xi_p1, xi_m1, grad_x_raw, grad_y_raw );
addCurvatureJacobian( 60.0, xi, xi_p1, xi_m1, curvature_params, grad_x_raw, grad_y_raw );
jacobians[0][0] = 0;
jacobians[1][0] = 0;
jacobians[0][0] = grad_x_raw; // x derivative
jacobians[1][0] = grad_y_raw; // y derivative
jacobians[0][1] = 0.0;
jacobians[1][1] = 0.0;
}
return true;
}
protected:
/**
* @brief Cost function term for smooth paths
* @param weight Weight to apply to function
* @param pt Point Xi for evaluation
* @param pt Point Xi+1 for calculating Xi's cost
* @param pt Point Xi-1 for calculating Xi's cost
* @param r Residual ( cost ) of term
*/
inline void addSmoothingResidual(
const double & weight,
const Eigen::Vector2d & pt,
const Eigen::Vector2d & pt_p,
const Eigen::Vector2d & pt_m,
double & r ) const
{
r += weight * (
pt_p.dot( pt_p ) -
4 * pt_p.dot( pt ) +
2 * pt_p.dot( pt_m ) +
4 * pt.dot( pt ) -
4 * pt.dot( pt_m ) +
pt_m.dot( pt_m ) ); // objective function value
}
/**
* @brief Cost function derivative term for smooth paths
* @param weight Weight to apply to function
* @param pt Point Xi for evaluation
* @param pt Point Xi+1 for calculating Xi's cost
* @param pt Point Xi-1 for calculating Xi's cost
* @param j0 Gradient of X term
* @param j1 Gradient of Y term
*/
inline void addSmoothingJacobian(
const double & weight,
const Eigen::Vector2d & pt,
const Eigen::Vector2d & pt_p,
const Eigen::Vector2d & pt_m,
double & j0,
double & j1 ) const
{
j0 += weight *
( -4 * pt_m[0] + 8 * pt[0] - 4 * pt_p[0] ); // xi x component of partial-derivative
j1 += weight *
( -4 * pt_m[1] + 8 * pt[1] - 4 * pt_p[1] ); // xi y component of partial-derivative
}
/**
* @brief Get path curvature information
* @param pt Point Xi for evaluation
* @param pt Point Xi+1 for calculating Xi's cost
* @param pt Point Xi-1 for calculating Xi's cost
* @param curvature_params A struct to cache computations for the jacobian to use
*/
inline void getCurvatureParams(
const Eigen::Vector2d & pt,
const Eigen::Vector2d & pt_p,
const Eigen::Vector2d & pt_m,
CurvatureComputations & curvature_params ) const
{
curvature_params.valid = true;
curvature_params.delta_xi = Eigen::Vector2d( pt[0] - pt_m[0], pt[1] - pt_m[1] );
curvature_params.delta_xi_p = Eigen::Vector2d( pt_p[0] - pt[0], pt_p[1] - pt[1] );
curvature_params.delta_xi_norm = curvature_params.delta_xi.norm( );
curvature_params.delta_xi_p_norm = curvature_params.delta_xi_p.norm( );
if ( curvature_params.delta_xi_norm < EPSILON || curvature_params.delta_xi_p_norm < EPSILON ||
std::isnan( curvature_params.delta_xi_p_norm ) || std::isnan( curvature_params.delta_xi_norm ) ||
std::isinf( curvature_params.delta_xi_p_norm ) || std::isinf( curvature_params.delta_xi_norm ) )
{
// ensure we have non-nan values returned
curvature_params.valid = false;
return;
}
const double & delta_xi_by_xi_p =
curvature_params.delta_xi_norm * curvature_params.delta_xi_p_norm;
double projection =
curvature_params.delta_xi.dot( curvature_params.delta_xi_p ) / delta_xi_by_xi_p;
if ( fabs( 1 - projection ) < EPSILON || fabs( projection + 1 ) < EPSILON ) {
projection = 1.0;
}
curvature_params.delta_phi_i = std::acos( projection );
curvature_params.turning_rad = curvature_params.delta_phi_i / curvature_params.delta_xi_norm;
curvature_params.ki_minus_kmax = curvature_params.turning_rad - _upsample_ratio *
_params.max_curvature;
if ( curvature_params.ki_minus_kmax <= EPSILON ) {
// Quadratic penalty need not apply
curvature_params.valid = false;
}
}
/**
* @brief Cost function term for maximum curved paths
* @param weight Weight to apply to function
* @param pt Point Xi for evaluation
* @param pt Point Xi+1 for calculating Xi's cost
* @param pt Point Xi-1 for calculating Xi's cost
* @param curvature_params A struct to cache computations for the jacobian to use
* @param r Residual ( cost ) of term
*/
inline void addCurvatureResidual(
const double & weight,
const Eigen::Vector2d & pt,
const Eigen::Vector2d & pt_p,
const Eigen::Vector2d & pt_m,
CurvatureComputations & curvature_params,
double & r ) const
{
getCurvatureParams( pt, pt_p, pt_m, curvature_params );
if ( !curvature_params.isValid( ) ) {
return;
}
// objective function value
r += weight *
curvature_params.ki_minus_kmax * curvature_params.ki_minus_kmax;
}
/**
* @brief Cost function derivative term for maximum curvature paths
* @param weight Weight to apply to function
* @param pt Point Xi for evaluation
* @param pt Point Xi+1 for calculating Xi's cost
* @param pt Point Xi-1 for calculating Xi's cost
* @param curvature_params A struct with cached values to speed up Jacobian computation
* @param j0 Gradient of X term
* @param j1 Gradient of Y term
*/
inline void addCurvatureJacobian(
const double & weight,
const Eigen::Vector2d & pt,
const Eigen::Vector2d & pt_p,
const Eigen::Vector2d & /*pt_m*/,
CurvatureComputations & curvature_params,
double & j0,
double & j1 ) const
{
if ( !curvature_params.isValid( ) ) {
return;
}
const double & partial_delta_phi_i_wrt_cost_delta_phi_i =
-1 / std::sqrt( 1 - std::pow( std::cos( curvature_params.delta_phi_i ), 2 ) );
// const Eigen::Vector2d ones = Eigen::Vector2d( 1.0, 1.0 );
auto neg_pt_plus = -1 * pt_p;
Eigen::Vector2d p1 = normalizedOrthogonalComplement(
pt, neg_pt_plus, curvature_params.delta_xi_norm, curvature_params.delta_xi_p_norm );
Eigen::Vector2d p2 = normalizedOrthogonalComplement(
neg_pt_plus, pt, curvature_params.delta_xi_p_norm, curvature_params.delta_xi_norm );
const double & u = 2 * curvature_params.ki_minus_kmax;
const double & common_prefix =
( 1 / curvature_params.delta_xi_norm ) * partial_delta_phi_i_wrt_cost_delta_phi_i;
const double & common_suffix = curvature_params.delta_phi_i /
( curvature_params.delta_xi_norm * curvature_params.delta_xi_norm );
const Eigen::Vector2d & d_delta_xi_d_xi = curvature_params.delta_xi /
curvature_params.delta_xi_norm;
const Eigen::Vector2d jacobian = u *
( common_prefix * ( -p1 - p2 ) - ( common_suffix * d_delta_xi_d_xi ) );
const Eigen::Vector2d jacobian_im1 = u *
( common_prefix * p2 + ( common_suffix * d_delta_xi_d_xi ) );
const Eigen::Vector2d jacobian_ip1 = u * ( common_prefix * p1 );
j0 += weight * jacobian[0]; // xi x component of partial-derivative
j1 += weight * jacobian[1]; // xi y component of partial-derivative
// j0 += weight *
// ( jacobian_im1[0] + 2 * jacobian[0] + jacobian_ip1[0] );
// j1 += weight *
// ( jacobian_im1[1] + 2 * jacobian[1] + jacobian_ip1[1] );
}
/**
* @brief Computing the normalized orthogonal component of 2 vectors
* @param a Vector
* @param b Vector
* @param norm a Vector's norm
* @param norm b Vector's norm
* @return Normalized vector of orthogonal components
*/
inline Eigen::Vector2d normalizedOrthogonalComplement(
const Eigen::Vector2d & a,
const Eigen::Vector2d & b,
const double & a_norm,
const double & b_norm ) const
{
return ( a - ( a.dot( b ) * b / b.squaredNorm( ) ) ) / ( a_norm * b_norm );
}
std::vector<Eigen::Vector2d> _path;
SmootherParams _params;
int _upsample_ratio;
int index;
};
} // namespace nav2_smac_planner
#endif // DEPRECATED__UPSAMPLER_COST_FUNCTION_NLLS_HPP_
1 // Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <math.h>
#include <memory>
#include <string>
#include <vector>
#include <limits>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/costmap_subscriber.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_smac_planner/node_hybrid.hpp"
#include "nav2_smac_planner/node_lattice.hpp"
#include "nav2_smac_planner/a_star.hpp"
#include "nav2_smac_planner/collision_checker.hpp"
#include "ament_index_cpp/get_package_share_directory.hpp"
32 class RclCppFixture
{
public:
35 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
36 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
40 TEST( AStarTest, test_a_star_2d )
{
nav2_smac_planner::SearchInfo info;
nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::Node2D> a_star(
nav2_smac_planner::MotionModel::MOORE, info );
int max_iterations = 10000;
float tolerance = 0.0;
float some_tolerance = 20.0;
int it_on_approach = 10;
double max_planning_time = 120.0;
int num_it = 0;
a_star.initialize( false, max_iterations, it_on_approach, max_planning_time, 0.0, 1 );
nav2_costmap_2d::Costmap2D * costmapA =
new nav2_costmap_2d::Costmap2D( 100, 100, 0.1, 0.0, 0.0, 0 );
// island in the middle of lethal cost to cross
for ( unsigned int i = 40; i <= 60; ++i ) {
for ( unsigned int j = 40; j <= 60; ++j ) {
costmapA->setCost( i, j, 254 );
}
}
// functional case testing
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
std::make_unique<nav2_smac_planner::GridCollisionChecker>( costmapA, 1 );
checker->setFootprint( nav2_costmap_2d::Footprint( ), true, 0.0 );
a_star.setCollisionChecker( checker.get( ) );
a_star.setStart( 20u, 20u, 0 );
a_star.setGoal( 80u, 80u, 0 );
nav2_smac_planner::Node2D::CoordinateVector path;
EXPECT_TRUE( a_star.createPath( path, num_it, tolerance ) );
EXPECT_EQ( num_it, 102 );
// check path is the right size and collision free
EXPECT_EQ( path.size( ), 81u );
for ( unsigned int i = 0; i != path.size( ); i++ ) {
EXPECT_EQ( costmapA->getCost( path[i].x, path[i].y ), 0 );
}
// setting non-zero dim 3 for 2D search
EXPECT_THROW( a_star.setGoal( 0, 0, 10 ), std::runtime_error );
EXPECT_THROW( a_star.setStart( 0, 0, 10 ), std::runtime_error );
path.clear( );
// failure cases with invalid inputs
nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::Node2D> a_star_2(
nav2_smac_planner::MotionModel::VON_NEUMANN, info );
a_star_2.initialize( false, max_iterations, it_on_approach, max_planning_time, 0, 1 );
num_it = 0;
EXPECT_THROW( a_star_2.createPath( path, num_it, tolerance ), std::runtime_error );
a_star_2.setCollisionChecker( checker.get( ) );
num_it = 0;
EXPECT_THROW( a_star_2.createPath( path, num_it, tolerance ), std::runtime_error );
a_star_2.setStart( 50, 50, 0 ); // invalid
a_star_2.setGoal( 0, 0, 0 ); // valid
num_it = 0;
EXPECT_THROW( a_star_2.createPath( path, num_it, tolerance ), std::runtime_error );
a_star_2.setStart( 0, 0, 0 ); // valid
a_star_2.setGoal( 50, 50, 0 ); // invalid
num_it = 0;
EXPECT_THROW( a_star_2.createPath( path, num_it, tolerance ), std::runtime_error );
num_it = 0;
// invalid goal but liberal tolerance
a_star_2.setStart( 20, 20, 0 ); // valid
a_star_2.setGoal( 50, 50, 0 ); // invalid
EXPECT_TRUE( a_star_2.createPath( path, num_it, some_tolerance ) );
EXPECT_EQ( path.size( ), 42u );
for ( unsigned int i = 0; i != path.size( ); i++ ) {
EXPECT_EQ( costmapA->getCost( path[i].x, path[i].y ), 0 );
}
EXPECT_TRUE( a_star_2.getStart( ) != nullptr );
EXPECT_TRUE( a_star_2.getGoal( ) != nullptr );
EXPECT_EQ( a_star_2.getSizeX( ), 100u );
EXPECT_EQ( a_star_2.getSizeY( ), 100u );
EXPECT_EQ( a_star_2.getSizeDim3( ), 1u );
EXPECT_EQ( a_star_2.getToleranceHeuristic( ), 20.0 );
EXPECT_EQ( a_star_2.getOnApproachMaxIterations( ), 10 );
delete costmapA;
}
123 TEST( AStarTest, test_a_star_se2 )
{
nav2_smac_planner::SearchInfo info;
info.change_penalty = 0.1;
info.non_straight_penalty = 1.1;
info.reverse_penalty = 2.0;
info.minimum_turning_radius = 8; // in grid coordinates
info.retrospective_penalty = 0.015;
info.analytic_expansion_max_length = 20.0; // in grid coordinates
info.analytic_expansion_ratio = 3.5;
unsigned int size_theta = 72;
info.cost_penalty = 1.7;
nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::NodeHybrid> a_star(
nav2_smac_planner::MotionModel::DUBIN, info );
int max_iterations = 10000;
float tolerance = 10.0;
int it_on_approach = 10;
double max_planning_time = 120.0;
int num_it = 0;
a_star.initialize( false, max_iterations, it_on_approach, max_planning_time, 401, size_theta );
nav2_costmap_2d::Costmap2D * costmapA =
new nav2_costmap_2d::Costmap2D( 100, 100, 0.1, 0.0, 0.0, 0 );
// island in the middle of lethal cost to cross
for ( unsigned int i = 40; i <= 60; ++i ) {
for ( unsigned int j = 40; j <= 60; ++j ) {
costmapA->setCost( i, j, 254 );
}
}
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
std::make_unique<nav2_smac_planner::GridCollisionChecker>( costmapA, size_theta );
checker->setFootprint( nav2_costmap_2d::Footprint( ), true, 0.0 );
// functional case testing
a_star.setCollisionChecker( checker.get( ) );
a_star.setStart( 10u, 10u, 0u );
a_star.setGoal( 80u, 80u, 40u );
nav2_smac_planner::NodeHybrid::CoordinateVector path;
EXPECT_TRUE( a_star.createPath( path, num_it, tolerance ) );
// check path is the right size and collision free
EXPECT_EQ( num_it, 3222 );
EXPECT_EQ( path.size( ), 62u );
for ( unsigned int i = 0; i != path.size( ); i++ ) {
EXPECT_EQ( costmapA->getCost( path[i].x, path[i].y ), 0 );
}
// no skipped nodes
for ( unsigned int i = 1; i != path.size( ); i++ ) {
EXPECT_LT( hypotf( path[i].x - path[i - 1].x, path[i].y - path[i - 1].y ), 2.1f );
}
delete costmapA;
}
179 TEST( AStarTest, test_a_star_lattice )
{
nav2_smac_planner::SearchInfo info;
info.change_penalty = 0.05;
info.non_straight_penalty = 1.05;
info.reverse_penalty = 2.0;
info.retrospective_penalty = 0.1;
info.analytic_expansion_ratio = 3.5;
info.lattice_filepath =
ament_index_cpp::get_package_share_directory( "nav2_smac_planner" ) +
"/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
"/output.json";
info.minimum_turning_radius = 8; // in grid coordinates 0.4/0.05
info.analytic_expansion_max_length = 20.0; // in grid coordinates
unsigned int size_theta = 16;
info.cost_penalty = 2.0;
nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::NodeLattice> a_star(
nav2_smac_planner::MotionModel::STATE_LATTICE, info );
int max_iterations = 10000;
float tolerance = 10.0;
int it_on_approach = 10;
double max_planning_time = 120.0;
int num_it = 0;
a_star.initialize(
false, max_iterations, std::numeric_limits<int>::max( ), max_planning_time, 401, size_theta );
nav2_costmap_2d::Costmap2D * costmapA =
new nav2_costmap_2d::Costmap2D( 100, 100, 0.05, 0.0, 0.0, 0 );
// island in the middle of lethal cost to cross
for ( unsigned int i = 20; i <= 30; ++i ) {
for ( unsigned int j = 20; j <= 30; ++j ) {
costmapA->setCost( i, j, 254 );
}
}
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
std::make_unique<nav2_smac_planner::GridCollisionChecker>( costmapA, size_theta );
checker->setFootprint( nav2_costmap_2d::Footprint( ), true, 0.0 );
// functional case testing
a_star.setCollisionChecker( checker.get( ) );
a_star.setStart( 5u, 5u, 0u );
a_star.setGoal( 40u, 40u, 1u );
nav2_smac_planner::NodeLattice::CoordinateVector path;
EXPECT_TRUE( a_star.createPath( path, num_it, tolerance ) );
// check path is the right size and collision free
EXPECT_EQ( num_it, 21 );
EXPECT_EQ( path.size( ), 48u );
for ( unsigned int i = 0; i != path.size( ); i++ ) {
EXPECT_EQ( costmapA->getCost( path[i].x, path[i].y ), 0 );
}
// no skipped nodes
for ( unsigned int i = 1; i != path.size( ); i++ ) {
EXPECT_LT( hypotf( path[i].x - path[i - 1].x, path[i].y - path[i - 1].y ), 2.1f );
}
delete costmapA;
}
240 TEST( AStarTest, test_se2_single_pose_path )
{
nav2_smac_planner::SearchInfo info;
info.change_penalty = 0.1;
info.non_straight_penalty = 1.1;
info.reverse_penalty = 2.0;
info.retrospective_penalty = 0.0;
info.minimum_turning_radius = 8; // in grid coordinates
info.analytic_expansion_max_length = 20.0; // in grid coordinates
info.analytic_expansion_ratio = 3.5;
unsigned int size_theta = 72;
info.cost_penalty = 1.7;
nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::NodeHybrid> a_star(
nav2_smac_planner::MotionModel::DUBIN, info );
int max_iterations = 100;
float tolerance = 10.0;
int it_on_approach = 10;
double max_planning_time = 120.0;
int num_it = 0;
a_star.initialize( false, max_iterations, it_on_approach, max_planning_time, 401, size_theta );
nav2_costmap_2d::Costmap2D * costmapA =
new nav2_costmap_2d::Costmap2D( 100, 100, 0.1, 0.0, 0.0, 0 );
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
std::make_unique<nav2_smac_planner::GridCollisionChecker>( costmapA, size_theta );
checker->setFootprint( nav2_costmap_2d::Footprint( ), true, 0.0 );
// functional case testing
a_star.setCollisionChecker( checker.get( ) );
a_star.setStart( 10u, 10u, 0u );
// Goal is one costmap cell away
a_star.setGoal( 12u, 10u, 0u );
nav2_smac_planner::NodeHybrid::CoordinateVector path;
EXPECT_TRUE( a_star.createPath( path, num_it, tolerance ) );
// Check that the path is length one
// With the current implementation, this produces a longer path
// EXPECT_EQ( path.size( ), 1u );
EXPECT_GE( path.size( ), 1u );
delete costmapA;
}
285 TEST( AStarTest, test_constants )
{
nav2_smac_planner::MotionModel mm = nav2_smac_planner::MotionModel::UNKNOWN; // unknown
EXPECT_EQ( nav2_smac_planner::toString( mm ), std::string( "Unknown" ) );
mm = nav2_smac_planner::MotionModel::VON_NEUMANN; // vonneumann
EXPECT_EQ( nav2_smac_planner::toString( mm ), std::string( "Von Neumann" ) );
mm = nav2_smac_planner::MotionModel::MOORE; // moore
EXPECT_EQ( nav2_smac_planner::toString( mm ), std::string( "Moore" ) );
mm = nav2_smac_planner::MotionModel::DUBIN; // dubin
EXPECT_EQ( nav2_smac_planner::toString( mm ), std::string( "Dubin" ) );
mm = nav2_smac_planner::MotionModel::REEDS_SHEPP; // reeds-shepp
EXPECT_EQ( nav2_smac_planner::toString( mm ), std::string( "Reeds-Shepp" ) );
EXPECT_EQ(
nav2_smac_planner::fromString(
"VON_NEUMANN" ), nav2_smac_planner::MotionModel::VON_NEUMANN );
EXPECT_EQ( nav2_smac_planner::fromString( "MOORE" ), nav2_smac_planner::MotionModel::MOORE );
EXPECT_EQ( nav2_smac_planner::fromString( "DUBIN" ), nav2_smac_planner::MotionModel::DUBIN );
EXPECT_EQ(
nav2_smac_planner::fromString(
"REEDS_SHEPP" ), nav2_smac_planner::MotionModel::REEDS_SHEPP );
EXPECT_EQ( nav2_smac_planner::fromString( "NONE" ), nav2_smac_planner::MotionModel::UNKNOWN );
}
1 // Copyright ( c ) 2020 Shivang Patel
// Copyright ( c ) 2020 Samsung Research
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <vector>
#include <memory>
#include "gtest/gtest.h"
#include "nav2_smac_planner/collision_checker.hpp"
using namespace nav2_costmap_2d; // NOLINT
25 TEST( collision_footprint, test_basic )
{
nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D( 100, 100, 0.1, 0, 0, 0 );
geometry_msgs::msg::Point p1;
p1.x = -0.5;
p1.y = 0.0;
geometry_msgs::msg::Point p2;
p2.x = 0.0;
p2.y = 0.5;
geometry_msgs::msg::Point p3;
p3.x = 0.5;
p3.y = 0.0;
geometry_msgs::msg::Point p4;
p4.x = 0.0;
p4.y = -0.5;
nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
nav2_smac_planner::GridCollisionChecker collision_checker( costmap_, 72 );
collision_checker.setFootprint( footprint, false /*use footprint*/, 0.0 );
collision_checker.inCollision( 5.0, 5.0, 0.0, false );
float cost = collision_checker.getCost( );
EXPECT_NEAR( cost, 0.0, 0.001 );
delete costmap_;
}
52 TEST( collision_footprint, test_point_cost )
{
nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D( 100, 100, 0.1, 0, 0, 0 );
nav2_smac_planner::GridCollisionChecker collision_checker( costmap_, 72 );
nav2_costmap_2d::Footprint footprint;
collision_checker.setFootprint( footprint, true /*radius / pointcose*/, 0.0 );
collision_checker.inCollision( 5.0, 5.0, 0.0, false );
float cost = collision_checker.getCost( );
EXPECT_NEAR( cost, 0.0, 0.001 );
delete costmap_;
}
66 TEST( collision_footprint, test_world_to_map )
{
nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D( 100, 100, 0.1, 0, 0, 0 );
nav2_smac_planner::GridCollisionChecker collision_checker( costmap_, 72 );
nav2_costmap_2d::Footprint footprint;
collision_checker.setFootprint( footprint, true /*radius / point cost*/, 0.0 );
unsigned int x, y;
collision_checker.worldToMap( 1.0, 1.0, x, y );
collision_checker.inCollision( x, y, 0.0, false );
float cost = collision_checker.getCost( );
EXPECT_NEAR( cost, 0.0, 0.001 );
costmap_->setCost( 50, 50, 200 );
collision_checker.worldToMap( 5.0, 5.0, x, y );
collision_checker.inCollision( x, y, 0.0, false );
EXPECT_NEAR( collision_checker.getCost( ), 200.0, 0.001 );
delete costmap_;
}
91 TEST( collision_footprint, test_footprint_at_pose_with_movement )
{
nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D( 100, 100, 0.1, 0, 0, 254 );
for ( unsigned int i = 40; i <= 60; ++i ) {
for ( unsigned int j = 40; j <= 60; ++j ) {
costmap_->setCost( i, j, 128 );
}
}
geometry_msgs::msg::Point p1;
p1.x = -1.0;
p1.y = 1.0;
geometry_msgs::msg::Point p2;
p2.x = 1.0;
p2.y = 1.0;
geometry_msgs::msg::Point p3;
p3.x = 1.0;
p3.y = -1.0;
geometry_msgs::msg::Point p4;
p4.x = -1.0;
p4.y = -1.0;
nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
nav2_smac_planner::GridCollisionChecker collision_checker( costmap_, 72 );
collision_checker.setFootprint( footprint, false /*use footprint*/, 0.0 );
collision_checker.inCollision( 50, 50, 0.0, false );
float cost = collision_checker.getCost( );
EXPECT_NEAR( cost, 128.0, 0.001 );
collision_checker.inCollision( 50, 49, 0.0, false );
float up_value = collision_checker.getCost( );
EXPECT_NEAR( up_value, 254.0, 0.001 );
collision_checker.inCollision( 50, 52, 0.0, false );
float down_value = collision_checker.getCost( );
EXPECT_NEAR( down_value, 254.0, 0.001 );
delete costmap_;
}
133 TEST( collision_footprint, test_point_and_line_cost )
{
nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(
100, 100, 0.10000, 0, 0.0, 128.0 );
costmap_->setCost( 62, 50, 254 );
costmap_->setCost( 39, 60, 254 );
geometry_msgs::msg::Point p1;
p1.x = -1.0;
p1.y = 1.0;
geometry_msgs::msg::Point p2;
p2.x = 1.0;
p2.y = 1.0;
geometry_msgs::msg::Point p3;
p3.x = 1.0;
p3.y = -1.0;
geometry_msgs::msg::Point p4;
p4.x = -1.0;
p4.y = -1.0;
nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
nav2_smac_planner::GridCollisionChecker collision_checker( costmap_, 72 );
collision_checker.setFootprint( footprint, false /*use footprint*/, 0.0 );
collision_checker.inCollision( 50, 50, 0.0, false );
float value = collision_checker.getCost( );
EXPECT_NEAR( value, 128.0, 0.001 );
collision_checker.inCollision( 49, 50, 0.0, false );
float left_value = collision_checker.getCost( );
EXPECT_NEAR( left_value, 254.0, 0.001 );
collision_checker.inCollision( 52, 50, 0.0, false );
float right_value = collision_checker.getCost( );
EXPECT_NEAR( right_value, 254.0, 0.001 );
delete costmap_;
}
1 // Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <memory>
#include <vector>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_smac_planner/costmap_downsampler.hpp"
24 class RclCppFixture
{
public:
27 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
28 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
32 TEST( CostmapDownsampler, costmap_downsample_test )
{
nav2_util::LifecycleNode::SharedPtr node = std::make_shared<nav2_util::LifecycleNode>(
"CostmapDownsamplerTest" );
nav2_smac_planner::CostmapDownsampler downsampler;
// create basic costmap
nav2_costmap_2d::Costmap2D costmapA( 10, 10, 0.05, 0.0, 0.0, 0 );
costmapA.setCost( 0, 0, 100 );
costmapA.setCost( 5, 5, 50 );
// downsample it
downsampler.on_configure( node, "map", "unused_topic", &costmapA, 2 );
nav2_costmap_2d::Costmap2D * downsampledCostmapA = downsampler.downsample( 2 );
// validate it
EXPECT_EQ( downsampledCostmapA->getCost( 0, 0 ), 100 );
EXPECT_EQ( downsampledCostmapA->getCost( 2, 2 ), 50 );
EXPECT_EQ( downsampledCostmapA->getSizeInCellsX( ), 5u );
EXPECT_EQ( downsampledCostmapA->getSizeInCellsY( ), 5u );
// give it another costmap of another size
nav2_costmap_2d::Costmap2D costmapB( 4, 4, 0.10, 0.0, 0.0, 0 );
// downsample it
downsampler.on_configure( node, "map", "unused_topic", &costmapB, 4 );
downsampler.on_activate( );
nav2_costmap_2d::Costmap2D * downsampledCostmapB = downsampler.downsample( 4 );
downsampler.on_deactivate( );
// validate size
EXPECT_EQ( downsampledCostmapB->getSizeInCellsX( ), 1u );
EXPECT_EQ( downsampledCostmapB->getSizeInCellsY( ), 1u );
downsampler.resizeCostmap( );
}
1 // Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <memory>
#include <string>
#include <vector>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/costmap_subscriber.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_smac_planner/node_2d.hpp"
#include "nav2_smac_planner/collision_checker.hpp"
27 class RclCppFixture
{
public:
30 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
31 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
35 TEST( Node2DTest, test_node_2d )
{
nav2_costmap_2d::Costmap2D costmapA( 10, 10, 0.05, 0.0, 0.0, 0 );
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
std::make_unique<nav2_smac_planner::GridCollisionChecker>( &costmapA, 72 );
checker->setFootprint( nav2_costmap_2d::Footprint( ), true, 0.0 );
// test construction
unsigned char cost = static_cast<unsigned char>( 1 );
nav2_smac_planner::Node2D testA( 1 );
testA.setCost( cost );
nav2_smac_planner::Node2D testB( 1 );
testB.setCost( cost );
EXPECT_EQ( testA.getCost( ), 1.0f );
nav2_smac_planner::SearchInfo info;
info.cost_penalty = 1.0;
unsigned int size = 10;
nav2_smac_planner::Node2D::initMotionModel(
nav2_smac_planner::MotionModel::MOORE, size, size, size, info );
// test reset
testA.reset( );
EXPECT_TRUE( std::isnan( testA.getCost( ) ) );
// check collision checking
EXPECT_EQ( testA.isNodeValid( false, checker.get( ) ), true );
testA.setCost( 255 );
EXPECT_EQ( testA.isNodeValid( true, checker.get( ) ), true );
testA.setCost( 10 );
// check traversal cost computation
EXPECT_NEAR( testB.getTraversalCost( &testA ), 1.03f, 0.1f );
// check heuristic cost computation
nav2_smac_planner::Node2D::Coordinates A( 0.0, 0.0 );
nav2_smac_planner::Node2D::Coordinates B( 10.0, 5.0 );
EXPECT_NEAR( testB.getHeuristicCost( A, B, nullptr ), 15., 0.01 );
// check operator== works on index
unsigned char costC = '2';
nav2_smac_planner::Node2D testC( 1 );
testC.setCost( costC );
EXPECT_TRUE( testA == testC );
// check accumulated costs are set
testC.setAccumulatedCost( 100 );
EXPECT_EQ( testC.getAccumulatedCost( ), 100.0f );
// check visiting state
EXPECT_EQ( testC.wasVisited( ), false );
testC.queued( );
EXPECT_EQ( testC.isQueued( ), true );
testC.visited( );
EXPECT_EQ( testC.wasVisited( ), true );
EXPECT_EQ( testC.isQueued( ), false );
// check index
EXPECT_EQ( testC.getIndex( ), 1u );
// check static index functions
EXPECT_EQ( nav2_smac_planner::Node2D::getIndex( 1u, 1u, 10u ), 11u );
EXPECT_EQ( nav2_smac_planner::Node2D::getIndex( 6u, 43u, 10u ), 436u );
EXPECT_EQ( nav2_smac_planner::Node2D::getCoords( 436u, 10u, 1u ).x, 6u );
EXPECT_EQ( nav2_smac_planner::Node2D::getCoords( 436u, 10u, 1u ).y, 43u );
EXPECT_THROW( nav2_smac_planner::Node2D::getCoords( 436u, 10u, 10u ), std::runtime_error );
}
102 TEST( Node2DTest, test_node_2d_neighbors )
{
nav2_smac_planner::SearchInfo info;
unsigned int size_x = 10u;
unsigned int size_y = 10u;
unsigned int quant = 0u;
// test neighborhood computation
nav2_smac_planner::Node2D::initMotionModel(
nav2_smac_planner::MotionModel::VON_NEUMANN, size_x,
size_y, quant, info );
EXPECT_EQ( nav2_smac_planner::Node2D::_neighbors_grid_offsets.size( ), 4u );
EXPECT_EQ( nav2_smac_planner::Node2D::_neighbors_grid_offsets[0], -1 );
EXPECT_EQ( nav2_smac_planner::Node2D::_neighbors_grid_offsets[1], 1 );
EXPECT_EQ( nav2_smac_planner::Node2D::_neighbors_grid_offsets[2], -10 );
EXPECT_EQ( nav2_smac_planner::Node2D::_neighbors_grid_offsets[3], 10 );
size_x = 100u;
nav2_smac_planner::Node2D::initMotionModel(
nav2_smac_planner::MotionModel::MOORE, size_x, size_y,
quant, info );
EXPECT_EQ( nav2_smac_planner::Node2D::_neighbors_grid_offsets.size( ), 8u );
EXPECT_EQ( nav2_smac_planner::Node2D::_neighbors_grid_offsets[0], -1 );
EXPECT_EQ( nav2_smac_planner::Node2D::_neighbors_grid_offsets[1], 1 );
EXPECT_EQ( nav2_smac_planner::Node2D::_neighbors_grid_offsets[2], -100 );
EXPECT_EQ( nav2_smac_planner::Node2D::_neighbors_grid_offsets[3], 100 );
EXPECT_EQ( nav2_smac_planner::Node2D::_neighbors_grid_offsets[4], -101 );
EXPECT_EQ( nav2_smac_planner::Node2D::_neighbors_grid_offsets[5], -99 );
EXPECT_EQ( nav2_smac_planner::Node2D::_neighbors_grid_offsets[6], 99 );
EXPECT_EQ( nav2_smac_planner::Node2D::_neighbors_grid_offsets[7], 101 );
nav2_costmap_2d::Costmap2D costmapA( 10, 10, 0.05, 0.0, 0.0, 0 );
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
std::make_unique<nav2_smac_planner::GridCollisionChecker>( &costmapA, 72 );
unsigned char cost = static_cast<unsigned int>( 1 );
nav2_smac_planner::Node2D * node = new nav2_smac_planner::Node2D( 1 );
node->setCost( cost );
std::function<bool( const unsigned int &, nav2_smac_planner::Node2D * & )> neighborGetter =
[&, this]( const unsigned int & index, nav2_smac_planner::Node2D * & neighbor_rtn ) -> bool
{
return false;
};
nav2_smac_planner::Node2D::NodeVector neighbors;
node->getNeighbors( neighborGetter, checker.get( ), false, neighbors );
delete node;
// should be empty since totally invalid
EXPECT_EQ( neighbors.size( ), 0u );
}
1 // Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <math.h>
#include <memory>
#include <string>
#include <vector>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/costmap_subscriber.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_smac_planner/node_basic.hpp"
#include "nav2_smac_planner/node_2d.hpp"
#include "nav2_smac_planner/node_hybrid.hpp"
#include "nav2_smac_planner/node_lattice.hpp"
#include "nav2_smac_planner/collision_checker.hpp"
31 class RclCppFixture
{
public:
34 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
35 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
39 TEST( NodeBasicTest, test_node_basic )
{
nav2_smac_planner::NodeBasic<nav2_smac_planner::NodeHybrid> node( 50 );
EXPECT_EQ( node.index, 50u );
EXPECT_EQ( node.graph_node_ptr, nullptr );
nav2_smac_planner::NodeBasic<nav2_smac_planner::Node2D> node2( 100 );
EXPECT_EQ( node2.index, 100u );
EXPECT_EQ( node2.graph_node_ptr, nullptr );
nav2_smac_planner::NodeBasic<nav2_smac_planner::NodeLattice> node3( 200 );
EXPECT_EQ( node3.index, 200u );
EXPECT_EQ( node3.graph_node_ptr, nullptr );
}
1 // Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <math.h>
#include <memory>
#include <string>
#include <vector>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/costmap_subscriber.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_smac_planner/node_hybrid.hpp"
#include "nav2_smac_planner/collision_checker.hpp"
28 class RclCppFixture
{
public:
31 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
32 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
36 TEST( NodeHybridTest, test_node_hybrid )
{
nav2_smac_planner::SearchInfo info;
info.change_penalty = 0.1;
info.non_straight_penalty = 1.1;
info.reverse_penalty = 2.0;
info.minimum_turning_radius = 8; // 0.4m/5cm resolution costmap
info.cost_penalty = 1.7;
info.retrospective_penalty = 0.1;
unsigned int size_x = 10;
unsigned int size_y = 10;
unsigned int size_theta = 72;
// Check defaulted constants
nav2_smac_planner::NodeHybrid testA( 49 );
EXPECT_EQ( testA.travel_distance_cost, sqrt( 2 ) );
nav2_smac_planner::NodeHybrid::initMotionModel(
nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info );
nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(
10, 10, 0.05, 0.0, 0.0, 0 );
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
std::make_unique<nav2_smac_planner::GridCollisionChecker>( costmapA, 72 );
checker->setFootprint( nav2_costmap_2d::Footprint( ), true, 0.0 );
// test construction
nav2_smac_planner::NodeHybrid testB( 49 );
EXPECT_TRUE( std::isnan( testA.getCost( ) ) );
// test node valid and cost
testA.pose.x = 5;
testA.pose.y = 5;
testA.pose.theta = 0;
EXPECT_EQ( testA.isNodeValid( true, checker.get( ) ), true );
EXPECT_EQ( testA.isNodeValid( false, checker.get( ) ), true );
EXPECT_EQ( testA.getCost( ), 0.0f );
// test reset
testA.reset( );
EXPECT_TRUE( std::isnan( testA.getCost( ) ) );
// Check motion-specific constants
EXPECT_NEAR( testA.travel_distance_cost, 2.08842, 0.1 );
// check collision checking
EXPECT_EQ( testA.isNodeValid( false, checker.get( ) ), true );
// check traversal cost computation
// simulated first node, should return neutral cost
EXPECT_NEAR( testB.getTraversalCost( &testA ), 2.088, 0.1 );
// now with straight motion, cost is 0, so will be neutral as well
// but now reduced by retrospective penalty ( 10% )
testB.setMotionPrimitiveIndex( 1 );
testA.setMotionPrimitiveIndex( 0 );
EXPECT_NEAR( testB.getTraversalCost( &testA ), 2.088 * 0.9, 0.1 );
// same direction as parent, testB
testA.setMotionPrimitiveIndex( 1 );
EXPECT_NEAR( testB.getTraversalCost( &testA ), 2.297f * 0.9, 0.01 );
// opposite direction as parent, testB
testA.setMotionPrimitiveIndex( 2 );
EXPECT_NEAR( testB.getTraversalCost( &testA ), 2.506f * 0.9, 0.01 );
// will throw because never collision checked testB
EXPECT_THROW( testA.getTraversalCost( &testB ), std::runtime_error );
// check motion primitives
EXPECT_EQ( testA.getMotionPrimitiveIndex( ), 2u );
// check operator== works on index
nav2_smac_planner::NodeHybrid testC( 49 );
EXPECT_TRUE( testA == testC );
// check accumulated costs are set
testC.setAccumulatedCost( 100 );
EXPECT_EQ( testC.getAccumulatedCost( ), 100.0f );
// check visiting state
EXPECT_EQ( testC.wasVisited( ), false );
testC.visited( );
EXPECT_EQ( testC.wasVisited( ), true );
// check index
EXPECT_EQ( testC.getIndex( ), 49u );
// check set pose and pose
testC.setPose( nav2_smac_planner::NodeHybrid::Coordinates( 10.0, 5.0, 4 ) );
EXPECT_EQ( testC.pose.x, 10.0 );
EXPECT_EQ( testC.pose.y, 5.0 );
EXPECT_EQ( testC.pose.theta, 4 );
// check static index functions
EXPECT_EQ( nav2_smac_planner::NodeHybrid::getIndex( 1u, 1u, 4u, 10u, 72u ), 796u );
EXPECT_EQ( nav2_smac_planner::NodeHybrid::getCoords( 796u, 10u, 72u ).x, 1u );
EXPECT_EQ( nav2_smac_planner::NodeHybrid::getCoords( 796u, 10u, 72u ).y, 1u );
EXPECT_EQ( nav2_smac_planner::NodeHybrid::getCoords( 796u, 10u, 72u ).theta, 4u );
delete costmapA;
}
136 TEST( NodeHybridTest, test_obstacle_heuristic )
{
nav2_smac_planner::SearchInfo info;
info.change_penalty = 0.1;
info.non_straight_penalty = 1.1;
info.reverse_penalty = 2.0;
info.minimum_turning_radius = 8; // 0.4m/5cm resolution costmap
info.cost_penalty = 1.7;
info.retrospective_penalty = 0.0;
unsigned int size_x = 100;
unsigned int size_y = 100;
unsigned int size_theta = 72;
nav2_smac_planner::NodeHybrid::initMotionModel(
nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info );
nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(
100, 100, 0.1, 0.0, 0.0, 0 );
// island in the middle of lethal cost to cross
for ( unsigned int i = 20; i <= 80; ++i ) {
for ( unsigned int j = 40; j <= 60; ++j ) {
costmapA->setCost( i, j, 254 );
}
}
// path on the right is narrow and thus with high cost
for ( unsigned int i = 20; i <= 80; ++i ) {
for ( unsigned int j = 61; j <= 70; ++j ) {
costmapA->setCost( i, j, 250 );
}
}
for ( unsigned int i = 20; i <= 80; ++i ) {
for ( unsigned int j = 71; j < 100; ++j ) {
costmapA->setCost( i, j, 254 );
}
}
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
std::make_unique<nav2_smac_planner::GridCollisionChecker>( costmapA, 72 );
checker->setFootprint( nav2_costmap_2d::Footprint( ), true, 0.0 );
nav2_smac_planner::NodeHybrid testA( 0 );
testA.pose.x = 10;
testA.pose.y = 50;
testA.pose.theta = 0;
nav2_smac_planner::NodeHybrid testB( 1 );
testB.pose.x = 90;
testB.pose.y = 51; // goal is a bit closer to the high-cost passage
testB.pose.theta = 0;
// first block the high-cost passage to make sure the cost spreads through the better path
for ( unsigned int j = 61; j <= 70; ++j ) {
costmapA->setCost( 50, j, 254 );
}
nav2_smac_planner::NodeHybrid::resetObstacleHeuristic(
costmapA, testA.pose.x, testA.pose.y, testB.pose.x, testB.pose.y );
float wide_passage_cost = nav2_smac_planner::NodeHybrid::getObstacleHeuristic(
testA.pose,
testB.pose,
info.cost_penalty );
EXPECT_NEAR( wide_passage_cost, 91.1f, 0.1f );
// then unblock it to check if cost remains the same
// ( it should, since the unblocked narrow path will have higher cost than the wide one
// and thus lower bound of the path cost should be unchanged )
for ( unsigned int j = 61; j <= 70; ++j ) {
costmapA->setCost( 50, j, 250 );
}
nav2_smac_planner::NodeHybrid::resetObstacleHeuristic(
costmapA,
testA.pose.x, testA.pose.y, testB.pose.x, testB.pose.y );
float two_passages_cost = nav2_smac_planner::NodeHybrid::getObstacleHeuristic(
testA.pose,
testB.pose,
info.cost_penalty );
EXPECT_EQ( wide_passage_cost, two_passages_cost );
delete costmapA;
}
217 TEST( NodeHybridTest, test_node_debin_neighbors )
{
nav2_smac_planner::SearchInfo info;
info.change_penalty = 1.2;
info.non_straight_penalty = 1.4;
info.reverse_penalty = 2.1;
info.minimum_turning_radius = 4; // 0.2 in grid coordinates
info.retrospective_penalty = 0.0;
unsigned int size_x = 100;
unsigned int size_y = 100;
unsigned int size_theta = 72;
nav2_smac_planner::NodeHybrid::initMotionModel(
nav2_smac_planner::MotionModel::DUBIN, size_x, size_y, size_theta, info );
// test neighborhood computation
EXPECT_EQ( nav2_smac_planner::NodeHybrid::motion_table.projections.size( ), 3u );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[0]._x, 1.731517, 0.01 );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[0]._y, 0, 0.01 );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[0]._theta, 0, 0.01 );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[1]._x, 1.69047, 0.01 );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[1]._y, 0.3747, 0.01 );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[1]._theta, 5, 0.01 );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[2]._x, 1.69047, 0.01 );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[2]._y, -0.3747, 0.01 );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[2]._theta, -5, 0.01 );
}
246 TEST( NodeHybridTest, test_node_reeds_neighbors )
{
nav2_smac_planner::SearchInfo info;
info.change_penalty = 1.2;
info.non_straight_penalty = 1.4;
info.reverse_penalty = 2.1;
info.minimum_turning_radius = 8; // 0.4 in grid coordinates
info.retrospective_penalty = 0.0;
unsigned int size_x = 100;
unsigned int size_y = 100;
unsigned int size_theta = 72;
nav2_smac_planner::NodeHybrid::initMotionModel(
nav2_smac_planner::MotionModel::REEDS_SHEPP, size_x, size_y, size_theta, info );
EXPECT_EQ( nav2_smac_planner::NodeHybrid::motion_table.projections.size( ), 6u );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[0]._x, 2.088, 0.01 );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[0]._y, 0, 0.01 );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[0]._theta, 0, 0.01 );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[1]._x, 2.070, 0.01 );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[1]._y, 0.272, 0.01 );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[1]._theta, 3, 0.01 );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[2]._x, 2.070, 0.01 );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[2]._y, -0.272, 0.01 );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[2]._theta, -3, 0.01 );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[3]._x, -2.088, 0.01 );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[3]._y, 0, 0.01 );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[3]._theta, 0, 0.01 );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[4]._x, -2.07, 0.01 );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[4]._y, 0.272, 0.01 );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[4]._theta, -3, 0.01 );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[5]._x, -2.07, 0.01 );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[5]._y, -0.272, 0.01 );
EXPECT_NEAR( nav2_smac_planner::NodeHybrid::motion_table.projections[5]._theta, 3, 0.01 );
nav2_costmap_2d::Costmap2D costmapA( 100, 100, 0.05, 0.0, 0.0, 0 );
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
std::make_unique<nav2_smac_planner::GridCollisionChecker>( &costmapA, 72 );
checker->setFootprint( nav2_costmap_2d::Footprint( ), true, 0.0 );
nav2_smac_planner::NodeHybrid * node = new nav2_smac_planner::NodeHybrid( 49 );
std::function<bool( const unsigned int &, nav2_smac_planner::NodeHybrid * & )> neighborGetter =
[&, this]( const unsigned int & index, nav2_smac_planner::NodeHybrid * & neighbor_rtn ) -> bool
{
// because we don't return a real object
return false;
};
nav2_smac_planner::NodeHybrid::NodeVector neighbors;
node->getNeighbors( neighborGetter, checker.get( ), false, neighbors );
delete node;
// should be empty since totally invalid
EXPECT_EQ( neighbors.size( ), 0u );
}
1 // Copyright ( c ) 2021 Joshua Wallace
// Copyright ( c ) 2021 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <fstream>
#include <string>
#include <vector>
#include <memory>
#include <unordered_map>
#include <limits>
#include "nav2_smac_planner/node_lattice.hpp"
#include "gtest/gtest.h"
#include "ament_index_cpp/get_package_share_directory.hpp"
using json = nlohmann::json;
28 TEST( NodeLatticeTest, parser_test )
{
std::string pkg_share_dir = ament_index_cpp::get_package_share_directory( "nav2_smac_planner" );
std::string filePath =
pkg_share_dir +
"/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
"/output.json";
std::ifstream myJsonFile( filePath );
ASSERT_TRUE( myJsonFile.is_open( ) );
json j;
myJsonFile >> j;
nav2_smac_planner::LatticeMetadata metaData;
nav2_smac_planner::MotionPrimitive myPrimitive;
nav2_smac_planner::MotionPose pose;
json jsonMetaData = j["lattice_metadata"];
json jsonPrimatives = j["primitives"];
json jsonPose = jsonPrimatives[0]["poses"][0];
nav2_smac_planner::fromJsonToMetaData( jsonMetaData, metaData );
// Checks for parsing meta data
EXPECT_NEAR( metaData.min_turning_radius, 0.5, 0.001 );
EXPECT_NEAR( metaData.grid_resolution, 0.05, 0.001 );
EXPECT_NEAR( metaData.number_of_headings, 16, 0.01 );
EXPECT_NEAR( metaData.heading_angles[0], 0.0, 0.01 );
EXPECT_EQ( metaData.number_of_trajectories, 80u );
EXPECT_EQ( metaData.motion_model, std::string( "ackermann" ) );
std::vector<nav2_smac_planner::MotionPrimitive> myPrimitives;
for ( unsigned int i = 0; i < jsonPrimatives.size( ); ++i ) {
nav2_smac_planner::MotionPrimitive newPrimative;
nav2_smac_planner::fromJsonToMotionPrimitive( jsonPrimatives[i], newPrimative );
myPrimitives.push_back( newPrimative );
}
// Checks for parsing primitives
EXPECT_EQ( myPrimitives.size( ), 80u );
EXPECT_NEAR( myPrimitives[0].trajectory_id, 0, 0.01 );
EXPECT_NEAR( myPrimitives[0].start_angle, 0.0, 0.01 );
EXPECT_NEAR( myPrimitives[0].end_angle, 13, 0.01 );
EXPECT_NEAR( myPrimitives[0].turning_radius, 0.5259, 0.01 );
EXPECT_NEAR( myPrimitives[0].trajectory_length, 0.64856, 0.01 );
EXPECT_NEAR( myPrimitives[0].arc_length, 0.58225, 0.01 );
EXPECT_NEAR( myPrimitives[0].straight_length, 0.06631, 0.01 );
EXPECT_NEAR( myPrimitives[0].poses[0]._x, 0.04981, 0.01 );
EXPECT_NEAR( myPrimitives[0].poses[0]._y, -0.00236, 0.01 );
EXPECT_NEAR( myPrimitives[0].poses[0]._theta, 6.1883, 0.01 );
EXPECT_NEAR( myPrimitives[0].poses[1]._x, 0.09917, 0.01 );
EXPECT_NEAR( myPrimitives[0].poses[1]._y, -0.00944, 0.01 );
EXPECT_NEAR( myPrimitives[0].poses[1]._theta, 6.09345, 0.015 );
}
86 TEST( NodeLatticeTest, test_node_lattice_neighbors_and_parsing )
{
std::string pkg_share_dir = ament_index_cpp::get_package_share_directory( "nav2_smac_planner" );
std::string filePath =
pkg_share_dir +
"/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
"/output.json";
nav2_smac_planner::SearchInfo info;
info.minimum_turning_radius = 1.1;
info.non_straight_penalty = 1;
info.change_penalty = 1;
info.reverse_penalty = 1;
info.cost_penalty = 1;
info.retrospective_penalty = 0.0;
info.analytic_expansion_ratio = 1;
info.lattice_filepath = filePath;
info.cache_obstacle_heuristic = true;
info.allow_reverse_expansion = true;
unsigned int x = 100;
unsigned int y = 100;
unsigned int angle_quantization = 16;
nav2_smac_planner::NodeLattice::initMotionModel(
nav2_smac_planner::MotionModel::STATE_LATTICE, x, y, angle_quantization, info );
nav2_smac_planner::NodeLattice aNode( 0 );
aNode.setPose( nav2_smac_planner::NodeHybrid::Coordinates( 0, 0, 0 ) );
nav2_smac_planner::MotionPrimitivePtrs projections =
nav2_smac_planner::NodeLattice::motion_table.getMotionPrimitives( &aNode );
EXPECT_NEAR( projections[0]->poses.back( )._x, 0.5, 0.01 );
EXPECT_NEAR( projections[0]->poses.back( )._y, -0.35, 0.01 );
EXPECT_NEAR( projections[0]->poses.back( )._theta, 5.176, 0.01 );
EXPECT_NEAR(
nav2_smac_planner::NodeLattice::motion_table.getLatticeMetadata(
filePath ).grid_resolution, 0.05, 0.005 );
}
127 TEST( NodeLatticeTest, test_node_lattice_conversions )
{
std::string pkg_share_dir = ament_index_cpp::get_package_share_directory( "nav2_smac_planner" );
std::string filePath =
pkg_share_dir +
"/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
"/output.json";
nav2_smac_planner::SearchInfo info;
info.minimum_turning_radius = 1.1;
info.non_straight_penalty = 1;
info.change_penalty = 1;
info.reverse_penalty = 1;
info.cost_penalty = 1;
info.retrospective_penalty = 0.0;
info.analytic_expansion_ratio = 1;
info.lattice_filepath = filePath;
info.cache_obstacle_heuristic = true;
unsigned int x = 100;
unsigned int y = 100;
unsigned int angle_quantization = 16;
nav2_smac_planner::NodeLattice::initMotionModel(
nav2_smac_planner::MotionModel::STATE_LATTICE, x, y, angle_quantization, info );
nav2_smac_planner::NodeLattice aNode( 0 );
aNode.setPose( nav2_smac_planner::NodeHybrid::Coordinates( 0, 0, 0 ) );
EXPECT_NEAR( aNode.motion_table.getAngleFromBin( 0u ), 0.0, 0.005 );
EXPECT_NEAR( aNode.motion_table.getAngleFromBin( 1u ), 0.46364, 0.005 );
EXPECT_NEAR( aNode.motion_table.getAngleFromBin( 2u ), 0.78539, 0.005 );
EXPECT_EQ( aNode.motion_table.getClosestAngularBin( 0.0 ), 0u );
EXPECT_EQ( aNode.motion_table.getClosestAngularBin( 0.5 ), 1u );
EXPECT_EQ( aNode.motion_table.getClosestAngularBin( 1.5 ), 4u );
}
165 TEST( NodeLatticeTest, test_node_lattice )
{
std::string pkg_share_dir = ament_index_cpp::get_package_share_directory( "nav2_smac_planner" );
std::string filePath =
pkg_share_dir +
"/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
"/output.json";
nav2_smac_planner::SearchInfo info;
info.minimum_turning_radius = 1.1;
info.non_straight_penalty = 1;
info.change_penalty = 1;
info.reverse_penalty = 1;
info.cost_penalty = 1;
info.retrospective_penalty = 0.1;
info.analytic_expansion_ratio = 1;
info.lattice_filepath = filePath;
info.cache_obstacle_heuristic = true;
info.allow_reverse_expansion = true;
unsigned int x = 100;
unsigned int y = 100;
unsigned int angle_quantization = 16;
nav2_smac_planner::NodeLattice::initMotionModel(
nav2_smac_planner::MotionModel::STATE_LATTICE, x, y, angle_quantization, info );
// Check defaults
nav2_smac_planner::NodeLattice aNode( 0 );
nav2_smac_planner::NodeLattice testA( 49 );
EXPECT_EQ( testA.getIndex( ), 49u );
EXPECT_EQ( testA.getAccumulatedCost( ), std::numeric_limits<float>::max( ) );
EXPECT_TRUE( std::isnan( testA.getCost( ) ) );
EXPECT_EQ( testA.getMotionPrimitive( ), nullptr );
// Test visited state / reset
EXPECT_EQ( testA.wasVisited( ), false );
testA.visited( );
EXPECT_EQ( testA.wasVisited( ), true );
testA.reset( );
EXPECT_EQ( testA.wasVisited( ), false );
nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(
10, 10, 0.05, 0.0, 0.0, 0 );
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
std::make_unique<nav2_smac_planner::GridCollisionChecker>( costmapA, 72 );
checker->setFootprint( nav2_costmap_2d::Footprint( ), true, 0.0 );
// test node valid and cost
testA.pose.x = 5;
testA.pose.y = 5;
testA.pose.theta = 0;
EXPECT_EQ( testA.isNodeValid( true, checker.get( ) ), true );
EXPECT_EQ( testA.isNodeValid( false, checker.get( ) ), true );
EXPECT_EQ( testA.getCost( ), 0.0f );
// check collision checking
EXPECT_EQ( testA.isNodeValid( false, checker.get( ) ), true );
// check operator== works on index
nav2_smac_planner::NodeLattice testC( 49 );
EXPECT_TRUE( testA == testC );
// check accumulated costs are set
testC.setAccumulatedCost( 100 );
EXPECT_EQ( testC.getAccumulatedCost( ), 100.0f );
// check set pose and pose
testC.setPose( nav2_smac_planner::NodeLattice::Coordinates( 10.0, 5.0, 4 ) );
EXPECT_EQ( testC.pose.x, 10.0 );
EXPECT_EQ( testC.pose.y, 5.0 );
EXPECT_EQ( testC.pose.theta, 4 );
delete costmapA;
}
242 TEST( NodeLatticeTest, test_get_neighbors )
{
std::string pkg_share_dir = ament_index_cpp::get_package_share_directory( "nav2_smac_planner" );
std::string filePath =
pkg_share_dir +
"/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann" +
"/output.json";
nav2_smac_planner::SearchInfo info;
info.minimum_turning_radius = 1.1;
info.non_straight_penalty = 1;
info.change_penalty = 1;
info.reverse_penalty = 1;
info.cost_penalty = 1;
info.analytic_expansion_ratio = 1;
info.retrospective_penalty = 0.0;
info.lattice_filepath = filePath;
info.cache_obstacle_heuristic = true;
info.allow_reverse_expansion = true;
unsigned int x = 100;
unsigned int y = 100;
unsigned int angle_quantization = 16;
nav2_smac_planner::NodeLattice::initMotionModel(
nav2_smac_planner::MotionModel::STATE_LATTICE, x, y, angle_quantization, info );
nav2_smac_planner::NodeLattice node( 49 );
nav2_costmap_2d::Costmap2D * costmapA = new nav2_costmap_2d::Costmap2D(
10, 10, 0.05, 0.0, 0.0, 0 );
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
std::make_unique<nav2_smac_planner::GridCollisionChecker>( costmapA, 72 );
checker->setFootprint( nav2_costmap_2d::Footprint( ), true, 0.0 );
std::function<bool( const unsigned int &, nav2_smac_planner::NodeLattice * & )> neighborGetter =
[&, this]( const unsigned int & index, nav2_smac_planner::NodeLattice * & neighbor_rtn ) -> bool
{
// because we don't return a real object
return false;
};
nav2_smac_planner::NodeLattice::NodeVector neighbors;
node.getNeighbors( neighborGetter, checker.get( ), false, neighbors );
// should be empty since totally invalid
EXPECT_EQ( neighbors.size( ), 0u );
delete costmapA;
}
1 // Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <math.h>
#include <memory>
#include <string>
#include <vector>
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "gtest/gtest.h"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/costmap_subscriber.hpp"
#include "nav2_smac_planner/a_star.hpp"
#include "nav2_smac_planner/collision_checker.hpp"
#include "nav2_smac_planner/node_hybrid.hpp"
#include "nav2_smac_planner/smac_planner_2d.hpp"
#include "nav2_smac_planner/smac_planner_hybrid.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "rclcpp/rclcpp.hpp"
33 class RclCppFixture
{
public:
36 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
37 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
// SMAC smoke tests for plugin-level issues rather than algorithms
// ( covered by more extensively testing in other files )
// System tests in nav2_system_tests will actually plan with this work
45 TEST( SmacTest, test_smac_2d ) {
rclcpp_lifecycle::LifecycleNode::SharedPtr node2D =
std::make_shared<rclcpp_lifecycle::LifecycleNode>( "Smac2DTest" );
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
std::make_shared<nav2_costmap_2d::Costmap2DROS>( "global_costmap" );
costmap_ros->on_configure( rclcpp_lifecycle::State( ) );
node2D->declare_parameter( "test.smooth_path", true );
node2D->set_parameter( rclcpp::Parameter( "test.smooth_path", true ) );
node2D->declare_parameter( "test.downsample_costmap", true );
node2D->set_parameter( rclcpp::Parameter( "test.downsample_costmap", true ) );
node2D->declare_parameter( "test.downsampling_factor", 2 );
node2D->set_parameter( rclcpp::Parameter( "test.downsampling_factor", 2 ) );
geometry_msgs::msg::PoseStamped start, goal;
start.pose.position.x = 0.0;
start.pose.position.y = 0.0;
start.pose.orientation.w = 1.0;
// goal = start;
goal.pose.position.x = 7.0;
goal.pose.position.y = 0.0;
goal.pose.orientation.w = 1.0;
auto planner_2d = std::make_unique<nav2_smac_planner::SmacPlanner2D>( );
planner_2d->configure( node2D, "test", nullptr, costmap_ros );
planner_2d->activate( );
try {
planner_2d->createPlan( start, goal );
} catch ( ... ) {
}
planner_2d->deactivate( );
planner_2d->cleanup( );
planner_2d.reset( );
costmap_ros->on_cleanup( rclcpp_lifecycle::State( ) );
node2D.reset( );
costmap_ros.reset( );
}
85 TEST( SmacTest, test_smac_2d_reconfigure ) {
rclcpp_lifecycle::LifecycleNode::SharedPtr node2D =
std::make_shared<rclcpp_lifecycle::LifecycleNode>( "Smac2DTest" );
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
std::make_shared<nav2_costmap_2d::Costmap2DROS>( "global_costmap" );
costmap_ros->on_configure( rclcpp_lifecycle::State( ) );
auto planner_2d = std::make_unique<nav2_smac_planner::SmacPlanner2D>( );
planner_2d->configure( node2D, "test", nullptr, costmap_ros );
planner_2d->activate( );
auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
node2D->get_node_base_interface( ), node2D->get_node_topics_interface( ),
node2D->get_node_graph_interface( ),
node2D->get_node_services_interface( ) );
auto results = rec_param->set_parameters_atomically(
{rclcpp::Parameter( "test.tolerance", 1.0 ),
rclcpp::Parameter( "test.cost_travel_multiplier", 1.0 ),
rclcpp::Parameter( "test.max_planning_time", 2.0 ),
rclcpp::Parameter( "test.downsample_costmap", false ),
rclcpp::Parameter( "test.allow_unknown", false ),
rclcpp::Parameter( "test.downsampling_factor", 2 ),
rclcpp::Parameter( "test.max_iterations", -1 ),
rclcpp::Parameter( "test.max_on_approach_iterations", -1 ),
rclcpp::Parameter( "test.motion_model_for_search", "UNKNOWN" ),
rclcpp::Parameter( "test.use_final_approach_orientation", false )} );
rclcpp::spin_until_future_complete(
node2D->get_node_base_interface( ),
results );
EXPECT_EQ( node2D->get_parameter( "test.tolerance" ).as_double( ), 1.0 );
EXPECT_EQ(
node2D->get_parameter( "test.cost_travel_multiplier" ).as_double( ),
1.0 );
EXPECT_EQ( node2D->get_parameter( "test.max_planning_time" ).as_double( ), 2.0 );
EXPECT_EQ( node2D->get_parameter( "test.downsample_costmap" ).as_bool( ), false );
EXPECT_EQ( node2D->get_parameter( "test.allow_unknown" ).as_bool( ), false );
EXPECT_EQ( node2D->get_parameter( "test.downsampling_factor" ).as_int( ), 2 );
EXPECT_EQ( node2D->get_parameter( "test.max_iterations" ).as_int( ), -1 );
EXPECT_EQ( node2D->get_parameter( "test.use_final_approach_orientation" ).as_bool( ), false );
EXPECT_EQ(
node2D->get_parameter( "test.max_on_approach_iterations" ).as_int( ),
-1 );
EXPECT_EQ(
node2D->get_parameter( "test.motion_model_for_search" ).as_string( ),
"UNKNOWN" );
results = rec_param->set_parameters_atomically(
{rclcpp::Parameter( "test.downsample_costmap", true )} );
rclcpp::spin_until_future_complete(
node2D->get_node_base_interface( ),
results );
}
1 // Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <math.h>
#include <memory>
#include <string>
#include <vector>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/costmap_subscriber.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_smac_planner/node_hybrid.hpp"
#include "nav2_smac_planner/a_star.hpp"
#include "nav2_smac_planner/collision_checker.hpp"
#include "nav2_smac_planner/smac_planner_hybrid.hpp"
#include "nav2_smac_planner/smac_planner_2d.hpp"
32 class RclCppFixture
{
public:
35 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
36 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
// SMAC smoke tests for plugin-level issues rather than algorithms
// ( covered by more extensively testing in other files )
// System tests in nav2_system_tests will actually plan with this work
44 TEST( SmacTest, test_smac_se2 )
{
rclcpp_lifecycle::LifecycleNode::SharedPtr nodeSE2 =
std::make_shared<rclcpp_lifecycle::LifecycleNode>( "SmacSE2Test" );
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
std::make_shared<nav2_costmap_2d::Costmap2DROS>( "global_costmap" );
costmap_ros->on_configure( rclcpp_lifecycle::State( ) );
nodeSE2->declare_parameter( "test.downsample_costmap", true );
nodeSE2->set_parameter( rclcpp::Parameter( "test.downsample_costmap", true ) );
nodeSE2->declare_parameter( "test.downsampling_factor", 2 );
nodeSE2->set_parameter( rclcpp::Parameter( "test.downsampling_factor", 2 ) );
geometry_msgs::msg::PoseStamped start, goal;
start.pose.position.x = 0.0;
start.pose.position.y = 0.0;
start.pose.orientation.w = 1.0;
goal.pose.position.x = 1.0;
goal.pose.position.y = 1.0;
goal.pose.orientation.w = 1.0;
auto planner = std::make_unique<nav2_smac_planner::SmacPlannerHybrid>( );
planner->configure( nodeSE2, "test", nullptr, costmap_ros );
planner->activate( );
try {
planner->createPlan( start, goal );
} catch ( ... ) {
}
planner->deactivate( );
planner->cleanup( );
planner.reset( );
costmap_ros->on_cleanup( rclcpp_lifecycle::State( ) );
costmap_ros.reset( );
nodeSE2.reset( );
}
83 TEST( SmacTest, test_smac_se2_reconfigure )
{
rclcpp_lifecycle::LifecycleNode::SharedPtr nodeSE2 =
std::make_shared<rclcpp_lifecycle::LifecycleNode>( "SmacSE2Test" );
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
std::make_shared<nav2_costmap_2d::Costmap2DROS>( "global_costmap" );
costmap_ros->on_configure( rclcpp_lifecycle::State( ) );
auto planner = std::make_unique<nav2_smac_planner::SmacPlannerHybrid>( );
planner->configure( nodeSE2, "test", nullptr, costmap_ros );
planner->activate( );
auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
nodeSE2->get_node_base_interface( ), nodeSE2->get_node_topics_interface( ),
nodeSE2->get_node_graph_interface( ),
nodeSE2->get_node_services_interface( ) );
auto results = rec_param->set_parameters_atomically(
{rclcpp::Parameter( "test.downsample_costmap", true ),
rclcpp::Parameter( "test.downsampling_factor", 2 ),
rclcpp::Parameter( "test.angle_quantization_bins", 100 ),
rclcpp::Parameter( "test.allow_unknown", false ),
rclcpp::Parameter( "test.max_iterations", -1 ),
rclcpp::Parameter( "test.minimum_turning_radius", 1.0 ),
rclcpp::Parameter( "test.cache_obstacle_heuristic", true ),
rclcpp::Parameter( "test.reverse_penalty", 5.0 ),
rclcpp::Parameter( "test.change_penalty", 1.0 ),
rclcpp::Parameter( "test.non_straight_penalty", 2.0 ),
rclcpp::Parameter( "test.cost_penalty", 2.0 ),
rclcpp::Parameter( "test.retrospective_penalty", 0.2 ),
rclcpp::Parameter( "test.analytic_expansion_ratio", 4.0 ),
rclcpp::Parameter( "test.max_planning_time", 10.0 ),
rclcpp::Parameter( "test.lookup_table_size", 30.0 ),
rclcpp::Parameter( "test.smooth_path", false ),
rclcpp::Parameter( "test.analytic_expansion_max_length", 42.0 ),
rclcpp::Parameter( "test.motion_model_for_search", std::string( "REEDS_SHEPP" ) )} );
rclcpp::spin_until_future_complete(
nodeSE2->get_node_base_interface( ),
results );
EXPECT_EQ( nodeSE2->get_parameter( "test.downsample_costmap" ).as_bool( ), true );
EXPECT_EQ( nodeSE2->get_parameter( "test.downsampling_factor" ).as_int( ), 2 );
EXPECT_EQ( nodeSE2->get_parameter( "test.angle_quantization_bins" ).as_int( ), 100 );
EXPECT_EQ( nodeSE2->get_parameter( "test.allow_unknown" ).as_bool( ), false );
EXPECT_EQ( nodeSE2->get_parameter( "test.max_iterations" ).as_int( ), -1 );
EXPECT_EQ( nodeSE2->get_parameter( "test.minimum_turning_radius" ).as_double( ), 1.0 );
EXPECT_EQ( nodeSE2->get_parameter( "test.cache_obstacle_heuristic" ).as_bool( ), true );
EXPECT_EQ( nodeSE2->get_parameter( "test.reverse_penalty" ).as_double( ), 5.0 );
EXPECT_EQ( nodeSE2->get_parameter( "test.change_penalty" ).as_double( ), 1.0 );
EXPECT_EQ( nodeSE2->get_parameter( "test.non_straight_penalty" ).as_double( ), 2.0 );
EXPECT_EQ( nodeSE2->get_parameter( "test.cost_penalty" ).as_double( ), 2.0 );
EXPECT_EQ( nodeSE2->get_parameter( "test.retrospective_penalty" ).as_double( ), 0.2 );
EXPECT_EQ( nodeSE2->get_parameter( "test.analytic_expansion_ratio" ).as_double( ), 4.0 );
EXPECT_EQ( nodeSE2->get_parameter( "test.smooth_path" ).as_bool( ), false );
EXPECT_EQ( nodeSE2->get_parameter( "test.max_planning_time" ).as_double( ), 10.0 );
EXPECT_EQ( nodeSE2->get_parameter( "test.lookup_table_size" ).as_double( ), 30.0 );
EXPECT_EQ( nodeSE2->get_parameter( "test.analytic_expansion_max_length" ).as_double( ), 42.0 );
EXPECT_EQ(
nodeSE2->get_parameter( "test.motion_model_for_search" ).as_string( ),
std::string( "REEDS_SHEPP" ) );
}
1 // Copyright ( c ) 2021 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <math.h>
#include <memory>
#include <string>
#include <vector>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/costmap_subscriber.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_smac_planner/node_hybrid.hpp"
#include "nav2_smac_planner/a_star.hpp"
#include "nav2_smac_planner/collision_checker.hpp"
#include "nav2_smac_planner/smac_planner_lattice.hpp"
31 class RclCppFixture
{
public:
34 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
35 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
// Simple wrapper to be able to call a private member
40 class LatticeWrap : public nav2_smac_planner::SmacPlannerLattice
{
public:
43 void callDynamicParams( std::vector<rclcpp::Parameter> parameters )
{
dynamicParametersCallback( parameters );
}
};
// SMAC smoke tests for plugin-level issues rather than algorithms
// ( covered by more extensively testing in other files )
// System tests in nav2_system_tests will actually plan with this work
53 TEST( SmacTest, test_smac_lattice )
{
rclcpp_lifecycle::LifecycleNode::SharedPtr nodeLattice =
std::make_shared<rclcpp_lifecycle::LifecycleNode>( "SmacLatticeTest" );
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
std::make_shared<nav2_costmap_2d::Costmap2DROS>( "global_costmap" );
costmap_ros->on_configure( rclcpp_lifecycle::State( ) );
geometry_msgs::msg::PoseStamped start, goal;
start.pose.position.x = 0.0;
start.pose.position.y = 0.0;
start.pose.orientation.w = 1.0;
goal.pose.position.x = 1.0;
goal.pose.position.y = 1.0;
goal.pose.orientation.w = 1.0;
auto planner = std::make_unique<nav2_smac_planner::SmacPlannerLattice>( );
try {
// Expect to throw due to invalid prims file in param
planner->configure( nodeLattice, "test", nullptr, costmap_ros );
} catch ( ... ) {
}
planner->activate( );
try {
planner->createPlan( start, goal );
} catch ( ... ) {
}
planner->deactivate( );
planner->cleanup( );
planner.reset( );
costmap_ros->on_cleanup( rclcpp_lifecycle::State( ) );
costmap_ros.reset( );
nodeLattice.reset( );
}
91 TEST( SmacTest, test_smac_lattice_reconfigure )
{
rclcpp_lifecycle::LifecycleNode::SharedPtr nodeLattice =
std::make_shared<rclcpp_lifecycle::LifecycleNode>( "SmacLatticeTest" );
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
std::make_shared<nav2_costmap_2d::Costmap2DROS>( "global_costmap" );
costmap_ros->on_configure( rclcpp_lifecycle::State( ) );
auto planner = std::make_unique<LatticeWrap>( );
try {
// Expect to throw due to invalid prims file in param
planner->configure( nodeLattice, "test", nullptr, costmap_ros );
} catch ( ... ) {
}
planner->activate( );
auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
nodeLattice->get_node_base_interface( ), nodeLattice->get_node_topics_interface( ),
nodeLattice->get_node_graph_interface( ),
nodeLattice->get_node_services_interface( ) );
auto results = rec_param->set_parameters_atomically(
{rclcpp::Parameter( "test.allow_unknown", false ),
rclcpp::Parameter( "test.max_iterations", -1 ),
rclcpp::Parameter( "test.cache_obstacle_heuristic", true ),
rclcpp::Parameter( "test.reverse_penalty", 5.0 ),
rclcpp::Parameter( "test.change_penalty", 1.0 ),
rclcpp::Parameter( "test.non_straight_penalty", 2.0 ),
rclcpp::Parameter( "test.cost_penalty", 2.0 ),
rclcpp::Parameter( "test.retrospective_penalty", 0.2 ),
rclcpp::Parameter( "test.analytic_expansion_ratio", 4.0 ),
rclcpp::Parameter( "test.max_planning_time", 10.0 ),
rclcpp::Parameter( "test.lookup_table_size", 30.0 ),
rclcpp::Parameter( "test.smooth_path", false ),
rclcpp::Parameter( "test.analytic_expansion_max_length", 42.0 ),
rclcpp::Parameter( "test.rotation_penalty", 42.0 ),
rclcpp::Parameter( "test.allow_reverse_expansion", true )} );
try {
// All of these params will re-init A* which will involve loading the control set file
// which will cause an exception because the file does not exist. This will cause an
// expected failure preventing parameter updates from being successfully processed
rclcpp::spin_until_future_complete(
nodeLattice->get_node_base_interface( ),
results );
} catch ( ... ) {
}
// So instead, lets call manually on a change
std::vector<rclcpp::Parameter> parameters;
parameters.push_back( rclcpp::Parameter( "test.lattice_filepath", std::string( "HI" ) ) );
EXPECT_THROW( planner->callDynamicParams( parameters ), std::runtime_error );
}
1 // Copyright ( c ) 2020, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <math.h>
#include <memory>
#include <string>
#include <vector>
#include <limits>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/costmap_subscriber.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_smac_planner/node_hybrid.hpp"
#include "nav2_smac_planner/a_star.hpp"
#include "nav2_smac_planner/collision_checker.hpp"
#include "nav2_smac_planner/smoother.hpp"
#include "ament_index_cpp/get_package_share_directory.hpp"
using namespace nav2_smac_planner; // NOLINT
34 class RclCppFixture
{
public:
37 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
38 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
42 class SmootherWrapper : public nav2_smac_planner::Smoother
{
public:
45 explicit SmootherWrapper( const SmootherParams & params )
: nav2_smac_planner::Smoother( params )
{}
49 std::vector<PathSegment> findDirectionalPathSegmentsWrapper( nav_msgs::msg::Path path )
{
return findDirectionalPathSegments( path );
}
};
55 TEST( SmootherTest, test_full_smoother )
{
rclcpp_lifecycle::LifecycleNode::SharedPtr node =
std::make_shared<rclcpp_lifecycle::LifecycleNode>( "SmacSmootherTest" );
nav2_smac_planner::SmootherParams params;
params.get( node, "test" );
double maxtime = 1.0;
// Make smoother and costmap to smooth in
auto smoother = std::make_unique<SmootherWrapper>( params );
smoother->initialize( 0.4 /*turning radius*/ );
nav2_costmap_2d::Costmap2D * costmap =
new nav2_costmap_2d::Costmap2D( 100, 100, 0.05, 0.0, 0.0, 0 );
// island in the middle of lethal cost to cross
for ( unsigned int i = 20; i <= 30; ++i ) {
for ( unsigned int j = 20; j <= 30; ++j ) {
costmap->setCost( i, j, 254 );
}
}
// Setup A* search to get path to smooth
nav2_smac_planner::SearchInfo info;
info.change_penalty = 0.05;
info.non_straight_penalty = 1.05;
info.reverse_penalty = 2.0;
info.cost_penalty = 2.0;
info.retrospective_penalty = 0.0;
info.analytic_expansion_ratio = 3.5;
info.minimum_turning_radius = 8; // in grid coordinates 0.4/0.05
info.analytic_expansion_max_length = 20.0; // in grid coordinates
unsigned int size_theta = 72;
nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::NodeHybrid> a_star(
nav2_smac_planner::MotionModel::REEDS_SHEPP, info );
int max_iterations = 10000;
float tolerance = 10.0;
int it_on_approach = 10;
double max_planning_time = 120.0;
int num_it = 0;
a_star.initialize(
false, max_iterations, std::numeric_limits<int>::max( ), max_planning_time, 401, size_theta );
std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
std::make_unique<nav2_smac_planner::GridCollisionChecker>( costmap, size_theta );
checker->setFootprint( nav2_costmap_2d::Footprint( ), true, 0.0 );
// Create A* search to smooth
a_star.setCollisionChecker( checker.get( ) );
a_star.setStart( 5u, 5u, 0u );
a_star.setGoal( 45u, 45u, 36u );
nav2_smac_planner::NodeHybrid::CoordinateVector path;
EXPECT_TRUE( a_star.createPath( path, num_it, tolerance ) );
// Convert to world coordinates and get length to compare to smoothed length
nav_msgs::msg::Path plan;
plan.header.stamp = node->now( );
plan.header.frame_id = "map";
geometry_msgs::msg::PoseStamped pose;
pose.header = plan.header;
pose.pose.position.z = 0.0;
pose.pose.orientation.x = 0.0;
pose.pose.orientation.y = 0.0;
pose.pose.orientation.z = 0.0;
pose.pose.orientation.w = 1.0;
double initial_length = 0.0;
double x_m = path[path.size( ) - 1].x, y_m = path[path.size( ) - 1].y;
plan.poses.reserve( path.size( ) );
for ( int i = path.size( ) - 1; i >= 0; --i ) {
pose.pose = nav2_smac_planner::getWorldCoords( path[i].x, path[i].y, costmap );
pose.pose.orientation = nav2_smac_planner::getWorldOrientation( path[i].theta );
plan.poses.push_back( pose );
initial_length += hypot( path[i].x - x_m, path[i].y - y_m );
x_m = path[i].x;
y_m = path[i].y;
}
// Check that we accurately detect that this path has a reversing segment
EXPECT_EQ( smoother->findDirectionalPathSegmentsWrapper( plan ).size( ), 2u );
// Test smoother, should succeed with same number of points
// and shorter overall length, while still being collision free.
auto path_size_in = plan.poses.size( );
EXPECT_TRUE( smoother->smooth( plan, costmap, maxtime ) );
EXPECT_EQ( plan.poses.size( ), path_size_in ); // Should have same number of poses
double length = 0.0;
x_m = plan.poses[0].pose.position.x;
y_m = plan.poses[0].pose.position.y;
for ( unsigned int i = 0; i != plan.poses.size( ); i++ ) {
// Should be collision free
EXPECT_EQ( costmap->getCost( plan.poses[i].pose.position.x, plan.poses[i].pose.position.y ), 0 );
length += hypot( plan.poses[i].pose.position.x - x_m, plan.poses[i].pose.position.y - y_m );
x_m = plan.poses[i].pose.position.x;
y_m = plan.poses[i].pose.position.y;
}
EXPECT_LT( length, initial_length ); // Should be shorter
// Try again but with failure modes
// Failure mode: not enough iterations to complete
params.max_its_ = 0;
auto smoother_bypass = std::make_unique<SmootherWrapper>( params );
EXPECT_FALSE( smoother_bypass->smooth( plan, costmap, maxtime ) );
params.max_its_ = 1;
auto smoother_failure = std::make_unique<SmootherWrapper>( params );
EXPECT_FALSE( smoother_failure->smooth( plan, costmap, maxtime ) );
// Failure mode: Not enough time
double max_no_time = 0.0;
EXPECT_FALSE( smoother->smooth( plan, costmap, max_no_time ) );
// Failure mode: Path is in collision, do 2x to exercise overlapping point
// attempts to update orientation should also fail
pose.pose.position.x = 1.25;
pose.pose.position.y = 1.25;
plan.poses.push_back( pose );
plan.poses.push_back( pose );
EXPECT_FALSE( smoother->smooth( plan, costmap, maxtime ) );
EXPECT_NEAR( plan.poses.end( )[-2].pose.orientation.z, 1.0, 1e-3 );
EXPECT_NEAR( plan.poses.end( )[-2].pose.orientation.x, 0.0, 1e-3 );
EXPECT_NEAR( plan.poses.end( )[-2].pose.orientation.y, 0.0, 1e-3 );
EXPECT_NEAR( plan.poses.end( )[-2].pose.orientation.w, 0.0, 1e-3 );
delete costmap;
}
1 // Copyright ( c ) 2021 RoboTech Vision
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_SMOOTHER__NAV2_SMOOTHER_HPP_
#define NAV2_SMOOTHER__NAV2_SMOOTHER_HPP_
#include <memory>
#include <string>
#include <thread>
#include <unordered_map>
#include <vector>
#include "nav2_core/smoother.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav2_costmap_2d/costmap_subscriber.hpp"
#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
#include "nav2_costmap_2d/footprint_subscriber.hpp"
#include "nav2_msgs/action/smooth_path.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav2_util/simple_action_server.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "pluginlib/class_loader.hpp"
namespace nav2_smoother
{
/**
* @class nav2_smoother::SmootherServer
* @brief This class hosts variety of plugins of different algorithms to
* smooth or refine a path from the exposed SmoothPath action server.
*/
45 class SmootherServer : public nav2_util::LifecycleNode
{
public:
using SmootherMap = std::unordered_map<std::string, nav2_core::Smoother::Ptr>;
/**
* @brief A constructor for nav2_smoother::SmootherServer
* @param options Additional options to control creation of the node.
*/
explicit SmootherServer( const rclcpp::NodeOptions & options = rclcpp::NodeOptions( ) );
/**
* @brief Destructor for nav2_smoother::SmootherServer
*/
~SmootherServer( );
protected:
/**
* @brief Configures smoother parameters and member variables
*
* Configures smoother plugin and costmap; Initialize odom subscriber,
* velocity publisher and smooth path action server.
* @param state LifeCycle Node's state
* @return Success or Failure
* @throw pluginlib::PluginlibException When failed to initialize smoother
* plugin
*/
nav2_util::CallbackReturn on_configure( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Loads smoother plugins from parameter file
* @return bool if successfully loaded the plugins
*/
bool loadSmootherPlugins( );
/**
* @brief Activates member variables
*
* Activates smoother, costmap, velocity publisher and smooth path action
* server
* @param state LifeCycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_activate( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Deactivates member variables
*
* Deactivates smooth path action server, smoother, costmap and velocity
* publisher. Before calling deactivate state, velocity is being set to zero.
* @param state LifeCycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Calls clean up states and resets member variables.
*
* Smoother and costmap clean up state is called, and resets rest of the
* variables
* @param state LifeCycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Called when in Shutdown state
* @param state LifeCycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_shutdown( const rclcpp_lifecycle::State & state ) override;
using Action = nav2_msgs::action::SmoothPath;
using ActionServer = nav2_util::SimpleActionServer<Action>;
/**
* @brief SmoothPath action server callback. Handles action server updates and
* spins server until goal is reached
*
* Provides global path to smoother received from action client. Local
* section of the path is optimized using smoother.
* @throw nav2_core::PlannerException
*/
void smoothPlan( );
/**
* @brief Find the valid smoother ID name for the given request
*
* @param c_name The requested smoother name
* @param name Reference to the name to use for control if any valid available
* @return bool Whether it found a valid smoother to use
*/
bool findSmootherId( const std::string & c_name, std::string & name );
// Our action server implements the SmoothPath action
std::unique_ptr<ActionServer> action_server_;
// Transforms
std::shared_ptr<tf2_ros::Buffer> tf_;
std::shared_ptr<tf2_ros::TransformListener> transform_listener_;
// Publishers and subscribers
rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr plan_publisher_;
// Smoother Plugins
pluginlib::ClassLoader<nav2_core::Smoother> lp_loader_;
SmootherMap smoothers_;
std::vector<std::string> default_ids_;
std::vector<std::string> default_types_;
std::vector<std::string> smoother_ids_;
std::vector<std::string> smoother_types_;
std::string smoother_ids_concat_, current_smoother_;
// Utilities
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
rclcpp::Clock steady_clock_;
};
} // namespace nav2_smoother
#endif // NAV2_SMOOTHER__NAV2_SMOOTHER_HPP_
// Copyright ( c ) 2022, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef NAV2_SMOOTHER__SIMPLE_SMOOTHER_HPP_
#define NAV2_SMOOTHER__SIMPLE_SMOOTHER_HPP_
#include <cmath>
#include <vector>
#include <string>
#include <iostream>
#include <memory>
#include <queue>
#include <utility>
#include "nav2_core/smoother.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/cost_values.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav_msgs/msg/path.hpp"
#include "angles/angles.h"
#include "tf2/utils.h"
namespace nav2_smoother
{
/**
* @class nav2_smoother::PathSegment
* @brief A segment of a path in start/end indices
*/
struct PathSegment
{
unsigned int start;
unsigned int end;
};
typedef std::vector<geometry_msgs::msg::PoseStamped>::iterator PathIterator;
typedef std::vector<geometry_msgs::msg::PoseStamped>::reverse_iterator ReversePathIterator;
/**
* @class nav2_smoother::SimpleSmoother
* @brief A path smoother implementation
*/
55 class SimpleSmoother : public nav2_core::Smoother
{
public:
/**
* @brief A constructor for nav2_smoother::SimpleSmoother
*/
SimpleSmoother( ) = default;
/**
* @brief A destructor for nav2_smoother::SimpleSmoother
*/
~SimpleSmoother( ) override = default;
void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr &,
std::string name, std::shared_ptr<tf2_ros::Buffer>,
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber>,
std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> ) override;
/**
* @brief Method to cleanup resources.
*/
void cleanup( ) override {costmap_sub_.reset( );}
/**
* @brief Method to activate smoother and any threads involved in execution.
*/
void activate( ) override {}
/**
* @brief Method to deactivate smoother and any threads involved in execution.
*/
void deactivate( ) override {}
/**
* @brief Method to smooth given path
*
* @param path In-out path to be smoothed
* @param max_time Maximum duration smoothing should take
* @return If smoothing was completed ( true ) or interrupted by time limit ( false )
*/
bool smooth(
nav_msgs::msg::Path & path,
const rclcpp::Duration & max_time ) override;
protected:
/**
* @brief Smoother method - does the smoothing on a segment
* @param path Reference to path
* @param reversing_segment Return if this is a reversing segment
* @param costmap Pointer to minimal costmap
* @param max_time Maximum time to compute, stop early if over limit
* @return If smoothing was successful
*/
109 bool smoothImpl(
nav_msgs::msg::Path & path,
bool & reversing_segment,
const nav2_costmap_2d::Costmap2D * costmap,
const double & max_time );
/**
* @brief Get the field value for a given dimension
* @param msg Current pose to sample
* @param dim Dimension ID of interest
* @return dim value
*/
121 inline double getFieldByDim(
const geometry_msgs::msg::PoseStamped & msg,
const unsigned int & dim );
/**
* @brief Set the field value for a given dimension
* @param msg Current pose to sample
* @param dim Dimension ID of interest
* @param value to set the dimention to for the pose
*/
131 inline void setFieldByDim(
geometry_msgs::msg::PoseStamped & msg, const unsigned int dim,
const double & value );
/**
* @brief Finds the starting and end indices of path segments where
* the robot is traveling in the same direction ( e.g. forward vs reverse )
* @param path Path in which to look for cusps
* @return Set of index pairs for each segment of the path in a given direction
*/
141 std::vector<PathSegment> findDirectionalPathSegments( const nav_msgs::msg::Path & path );
/**
* @brief For a given path, update the path point orientations based on smoothing
* @param path Path to approximate the path orientation in
* @param reversing_segment Return if this is a reversing segment
*/
148 inline void updateApproximatePathOrientations(
nav_msgs::msg::Path & path,
bool & reversing_segment );
double tolerance_, data_w_, smooth_w_;
int max_its_, refinement_ctr_;
bool do_refinement_;
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
156 rclcpp::Logger logger_{rclcpp::get_logger( "SimpleSmoother" )};
};
} // namespace nav2_smoother
#endif // NAV2_SMOOTHER__SIMPLE_SMOOTHER_HPP_
1 // Copyright ( c ) 2021 RoboTech Vision
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include "nav2_smoother/nav2_smoother.hpp"
#include "rclcpp/rclcpp.hpp"
21 int main( int argc, char ** argv )
{
rclcpp::init( argc, argv );
auto node = std::make_shared<nav2_smoother::SmootherServer>( );
rclcpp::spin( node->get_node_base_interface( ) );
rclcpp::shutdown( );
return 0;
}
// Copyright ( c ) 2019 RoboTech Vision
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <chrono>
#include <limits>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "nav2_core/exceptions.hpp"
#include "nav2_smoother/nav2_smoother.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav_2d_utils/conversions.hpp"
#include "nav_2d_utils/tf_help.hpp"
#include "tf2_ros/create_timer_ros.h"
using namespace std::chrono_literals;
namespace nav2_smoother
{
36 SmootherServer::SmootherServer( const rclcpp::NodeOptions & options )
: LifecycleNode( "smoother_server", "", options ),
lp_loader_( "nav2_core", "nav2_core::Smoother" ),
default_ids_{"simple_smoother"},
40 default_types_{"nav2_smoother::SimpleSmoother"}
{
42 RCLCPP_INFO( get_logger( ), "Creating smoother server" );
44 declare_parameter(
"costmap_topic", rclcpp::ParameterValue(
std::string(
"global_costmap/costmap_raw" ) ) );
48 declare_parameter(
"footprint_topic",
rclcpp::ParameterValue(
std::string( "global_costmap/published_footprint" ) ) );
52 declare_parameter(
"robot_base_frame",
rclcpp::ParameterValue( std::string( "base_link" ) ) );
55 declare_parameter( "transform_tolerance", rclcpp::ParameterValue( 0.1 ) );
56 declare_parameter( "smoother_plugins", default_ids_ );
}
SmootherServer::~SmootherServer( )
{
smoothers_.clear( );
}
nav2_util::CallbackReturn
SmootherServer::on_configure( const rclcpp_lifecycle::State & )
{
RCLCPP_INFO( get_logger( ), "Configuring smoother server" );
auto node = shared_from_this( );
get_parameter( "smoother_plugins", smoother_ids_ );
if ( smoother_ids_ == default_ids_ ) {
for ( size_t i = 0; i < default_ids_.size( ); ++i ) {
nav2_util::declare_parameter_if_not_declared(
node, default_ids_[i] + ".plugin",
rclcpp::ParameterValue( default_types_[i] ) );
}
}
tf_ = std::make_shared<tf2_ros::Buffer>( get_clock( ) );
auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
get_node_base_interface( ), get_node_timers_interface( ) );
tf_->setCreateTimerInterface( timer_interface );
transform_listener_ = std::make_shared<tf2_ros::TransformListener>( *tf_ );
std::string costmap_topic, footprint_topic, robot_base_frame;
double transform_tolerance;
this->get_parameter( "costmap_topic", costmap_topic );
this->get_parameter( "footprint_topic", footprint_topic );
this->get_parameter( "transform_tolerance", transform_tolerance );
this->get_parameter( "robot_base_frame", robot_base_frame );
costmap_sub_ = std::make_shared<nav2_costmap_2d::CostmapSubscriber>(
shared_from_this( ), costmap_topic );
footprint_sub_ = std::make_shared<nav2_costmap_2d::FootprintSubscriber>(
shared_from_this( ), footprint_topic, *tf_, robot_base_frame, transform_tolerance );
collision_checker_ =
std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(
*costmap_sub_, *footprint_sub_, this->get_name( ) );
if ( !loadSmootherPlugins( ) ) {
return nav2_util::CallbackReturn::FAILURE;
}
// Initialize pubs & subs
plan_publisher_ = create_publisher<nav_msgs::msg::Path>( "plan_smoothed", 1 );
// Create the action server that we implement with our smoothPath method
action_server_ = std::make_unique<ActionServer>(
shared_from_this( ),
"smooth_path",
std::bind( &SmootherServer::smoothPlan, this ),
nullptr,
std::chrono::milliseconds( 500 ),
true );
return nav2_util::CallbackReturn::SUCCESS;
}
bool SmootherServer::loadSmootherPlugins( )
{
auto node = shared_from_this( );
smoother_types_.resize( smoother_ids_.size( ) );
for ( size_t i = 0; i != smoother_ids_.size( ); i++ ) {
try {
smoother_types_[i] =
nav2_util::get_plugin_type_param( node, smoother_ids_[i] );
nav2_core::Smoother::Ptr smoother =
lp_loader_.createUniqueInstance( smoother_types_[i] );
RCLCPP_INFO(
get_logger( ), "Created smoother : %s of type %s",
smoother_ids_[i].c_str( ), smoother_types_[i].c_str( ) );
smoother->configure(
node, smoother_ids_[i], tf_, costmap_sub_,
footprint_sub_ );
smoothers_.insert( {smoother_ids_[i], smoother} );
} catch ( const pluginlib::PluginlibException & ex ) {
RCLCPP_FATAL(
get_logger( ), "Failed to create smoother. Exception: %s",
ex.what( ) );
return false;
}
}
for ( size_t i = 0; i != smoother_ids_.size( ); i++ ) {
smoother_ids_concat_ += smoother_ids_[i] + std::string( " " );
}
RCLCPP_INFO(
get_logger( ), "Smoother Server has %s smoothers available.",
smoother_ids_concat_.c_str( ) );
return true;
}
nav2_util::CallbackReturn
SmootherServer::on_activate( const rclcpp_lifecycle::State & )
{
RCLCPP_INFO( get_logger( ), "Activating" );
plan_publisher_->on_activate( );
SmootherMap::iterator it;
for ( it = smoothers_.begin( ); it != smoothers_.end( ); ++it ) {
it->second->activate( );
}
action_server_->activate( );
// create bond connection
createBond( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
SmootherServer::on_deactivate( const rclcpp_lifecycle::State & )
{
RCLCPP_INFO( get_logger( ), "Deactivating" );
action_server_->deactivate( );
SmootherMap::iterator it;
for ( it = smoothers_.begin( ); it != smoothers_.end( ); ++it ) {
it->second->deactivate( );
}
plan_publisher_->on_deactivate( );
// destroy bond connection
destroyBond( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
SmootherServer::on_cleanup( const rclcpp_lifecycle::State & )
{
RCLCPP_INFO( get_logger( ), "Cleaning up" );
// Cleanup the helper classes
SmootherMap::iterator it;
for ( it = smoothers_.begin( ); it != smoothers_.end( ); ++it ) {
it->second->cleanup( );
}
smoothers_.clear( );
// Release any allocated resources
action_server_.reset( );
plan_publisher_.reset( );
transform_listener_.reset( );
tf_.reset( );
footprint_sub_.reset( );
costmap_sub_.reset( );
collision_checker_.reset( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
SmootherServer::on_shutdown( const rclcpp_lifecycle::State & )
{
RCLCPP_INFO( get_logger( ), "Shutting down" );
return nav2_util::CallbackReturn::SUCCESS;
}
bool SmootherServer::findSmootherId(
const std::string & c_name,
std::string & current_smoother )
{
if ( smoothers_.find( c_name ) == smoothers_.end( ) ) {
if ( smoothers_.size( ) == 1 && c_name.empty( ) ) {
RCLCPP_WARN_ONCE(
get_logger( ),
"No smoother was specified in action call."
" Server will use only plugin loaded %s. "
"This warning will appear once.",
smoother_ids_concat_.c_str( ) );
current_smoother = smoothers_.begin( )->first;
} else {
RCLCPP_ERROR(
get_logger( ),
"SmoothPath called with smoother name %s, "
"which does not exist. Available smoothers are: %s.",
c_name.c_str( ), smoother_ids_concat_.c_str( ) );
return false;
}
} else {
RCLCPP_DEBUG( get_logger( ), "Selected smoother: %s.", c_name.c_str( ) );
current_smoother = c_name;
}
return true;
}
void SmootherServer::smoothPlan( )
{
auto start_time = steady_clock_.now( );
RCLCPP_INFO( get_logger( ), "Received a path to smooth." );
auto result = std::make_shared<Action::Result>( );
try {
std::string c_name = action_server_->get_current_goal( )->smoother_id;
std::string current_smoother;
if ( findSmootherId( c_name, current_smoother ) ) {
current_smoother_ = current_smoother;
} else {
action_server_->terminate_current( );
return;
}
// Perform smoothing
auto goal = action_server_->get_current_goal( );
result->path = goal->path;
result->was_completed = smoothers_[current_smoother_]->smooth(
result->path, goal->max_smoothing_duration );
result->smoothing_duration = steady_clock_.now( ) - start_time;
if ( !result->was_completed ) {
RCLCPP_INFO(
get_logger( ),
"Smoother %s did not complete smoothing in specified time limit"
"( %lf seconds ) and was interrupted after %lf seconds",
current_smoother_.c_str( ),
rclcpp::Duration( goal->max_smoothing_duration ).seconds( ),
rclcpp::Duration( result->smoothing_duration ).seconds( ) );
}
plan_publisher_->publish( result->path );
// Check for collisions
if ( goal->check_for_collisions ) {
geometry_msgs::msg::Pose2D pose2d;
bool fetch_data = true;
for ( const auto & pose : result->path.poses ) {
pose2d.x = pose.pose.position.x;
pose2d.y = pose.pose.position.y;
pose2d.theta = tf2::getYaw( pose.pose.orientation );
if ( !collision_checker_->isCollisionFree( pose2d, fetch_data ) ) {
RCLCPP_ERROR(
get_logger( ),
"Smoothed path leads to a collision at x: %lf, y: %lf, theta: %lf",
pose2d.x, pose2d.y, pose2d.theta );
action_server_->terminate_current( result );
return;
}
fetch_data = false;
}
}
RCLCPP_DEBUG(
get_logger( ), "Smoother succeeded ( time: %lf ), setting result",
rclcpp::Duration( result->smoothing_duration ).seconds( ) );
action_server_->succeeded_current( result );
} catch ( nav2_core::PlannerException & e ) {
RCLCPP_ERROR( this->get_logger( ), e.what( ) );
action_server_->terminate_current( );
return;
}
}
} // namespace nav2_smoother
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE( nav2_smoother::SmootherServer )
1 // Copyright ( c ) 2022, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <vector>
#include <memory>
#include "nav2_smoother/simple_smoother.hpp"
namespace nav2_smoother
{
using namespace nav2_util::geometry_utils; // NOLINT
using namespace std::chrono; // NOLINT
using nav2_util::declare_parameter_if_not_declared;
25 void SimpleSmoother::configure(
26 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
27 std::string name, std::shared_ptr<tf2_ros::Buffer>/*tf*/,
28 std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub,
29 std::shared_ptr<nav2_costmap_2d::FootprintSubscriber>/*footprint_sub*/ )
{
costmap_sub_ = costmap_sub;
auto node = parent.lock( );
logger_ = node->get_logger( );
declare_parameter_if_not_declared(
node, name + ".tolerance", rclcpp::ParameterValue( 1e-10 ) );
declare_parameter_if_not_declared(
node, name + ".max_its", rclcpp::ParameterValue( 1000 ) );
declare_parameter_if_not_declared(
node, name + ".w_data", rclcpp::ParameterValue( 0.2 ) );
declare_parameter_if_not_declared(
node, name + ".w_smooth", rclcpp::ParameterValue( 0.3 ) );
declare_parameter_if_not_declared(
node, name + ".do_refinement", rclcpp::ParameterValue( true ) );
node->get_parameter( name + ".tolerance", tolerance_ );
node->get_parameter( name + ".max_its", max_its_ );
node->get_parameter( name + ".w_data", data_w_ );
node->get_parameter( name + ".w_smooth", smooth_w_ );
node->get_parameter( name + ".do_refinement", do_refinement_ );
}
54 bool SimpleSmoother::smooth(
55 nav_msgs::msg::Path & path,
56 const rclcpp::Duration & max_time )
{
auto costmap = costmap_sub_->getCostmap( );
refinement_ctr_ = 0;
steady_clock::time_point start = steady_clock::now( );
double time_remaining = max_time.seconds( );
bool success = true, reversing_segment;
nav_msgs::msg::Path curr_path_segment;
curr_path_segment.header = path.header;
std::vector<PathSegment> path_segments = findDirectionalPathSegments( path );
for ( unsigned int i = 0; i != path_segments.size( ); i++ ) {
if ( path_segments[i].end - path_segments[i].start > 9 ) {
// Populate path segment
curr_path_segment.poses.clear( );
std::copy(
path.poses.begin( ) + path_segments[i].start,
path.poses.begin( ) + path_segments[i].end + 1,
std::back_inserter( curr_path_segment.poses ) );
// Make sure we're still able to smooth with time remaining
steady_clock::time_point now = steady_clock::now( );
time_remaining = max_time.seconds( ) - duration_cast<duration<double>>( now - start ).count( );
// Smooth path segment naively
success = success && smoothImpl(
curr_path_segment, reversing_segment, costmap.get( ), time_remaining );
// Assemble the path changes to the main path
std::copy(
curr_path_segment.poses.begin( ),
curr_path_segment.poses.end( ),
path.poses.begin( ) + path_segments[i].start );
}
}
return success;
}
98 bool SimpleSmoother::smoothImpl(
99 nav_msgs::msg::Path & path,
100 bool & reversing_segment,
101 const nav2_costmap_2d::Costmap2D * costmap,
const double & max_time )
{
steady_clock::time_point a = steady_clock::now( );
rclcpp::Duration max_dur = rclcpp::Duration::from_seconds( max_time );
int its = 0;
double change = tolerance_;
const unsigned int & path_size = path.poses.size( );
double x_i, y_i, y_m1, y_ip1, y_i_org;
unsigned int mx, my;
nav_msgs::msg::Path new_path = path;
nav_msgs::msg::Path last_path = path;
while ( change >= tolerance_ ) {
its += 1;
change = 0.0;
// Make sure the smoothing function will converge
if ( its >= max_its_ ) {
RCLCPP_WARN(
logger_,
"Number of iterations has exceeded limit of %i.", max_its_ );
path = last_path;
updateApproximatePathOrientations( path, reversing_segment );
return false;
}
// Make sure still have time left to process
steady_clock::time_point b = steady_clock::now( );
rclcpp::Duration timespan( duration_cast<duration<double>>( b - a ) );
if ( timespan > max_dur ) {
RCLCPP_WARN(
logger_,
"Smoothing time exceeded allowed duration of %0.2f.", max_time );
path = last_path;
updateApproximatePathOrientations( path, reversing_segment );
return false;
}
for ( unsigned int i = 1; i != path_size - 1; i++ ) {
for ( unsigned int j = 0; j != 2; j++ ) {
x_i = getFieldByDim( path.poses[i], j );
y_i = getFieldByDim( new_path.poses[i], j );
y_m1 = getFieldByDim( new_path.poses[i - 1], j );
y_ip1 = getFieldByDim( new_path.poses[i + 1], j );
y_i_org = y_i;
// Smooth based on local 3 point neighborhood and original data locations
y_i += data_w_ * ( x_i - y_i ) + smooth_w_ * ( y_ip1 + y_m1 - ( 2.0 * y_i ) );
setFieldByDim( new_path.poses[i], j, y_i );
change += abs( y_i - y_i_org );
}
// validate update is admissible, only checks cost if a valid costmap pointer is provided
float cost = 0.0;
if ( costmap ) {
costmap->worldToMap(
getFieldByDim( new_path.poses[i], 0 ),
getFieldByDim( new_path.poses[i], 1 ),
mx, my );
cost = static_cast<float>( costmap->getCost( mx, my ) );
}
if ( cost > nav2_costmap_2d::MAX_NON_OBSTACLE && cost != nav2_costmap_2d::NO_INFORMATION ) {
RCLCPP_DEBUG(
rclcpp::get_logger( "SmacPlannerSmoother" ),
"Smoothing process resulted in an infeasible collision. "
"Returning the last path before the infeasibility was introduced." );
path = last_path;
updateApproximatePathOrientations( path, reversing_segment );
return false;
}
}
last_path = new_path;
}
// Lets do additional refinement, it shouldn't take more than a couple milliseconds
// but really puts the path quality over the top.
if ( do_refinement_ && refinement_ctr_ < 4 ) {
refinement_ctr_++;
smoothImpl( new_path, reversing_segment, costmap, max_time );
}
updateApproximatePathOrientations( new_path, reversing_segment );
path = new_path;
return true;
}
192 double SimpleSmoother::getFieldByDim(
193 const geometry_msgs::msg::PoseStamped & msg, const unsigned int & dim )
{
if ( dim == 0 ) {
return msg.pose.position.x;
} else if ( dim == 1 ) {
return msg.pose.position.y;
} else {
return msg.pose.position.z;
}
}
204 void SimpleSmoother::setFieldByDim(
205 geometry_msgs::msg::PoseStamped & msg, const unsigned int dim,
const double & value )
{
if ( dim == 0 ) {
msg.pose.position.x = value;
} else if ( dim == 1 ) {
msg.pose.position.y = value;
} else {
msg.pose.position.z = value;
}
}
217 std::vector<PathSegment> SimpleSmoother::findDirectionalPathSegments(
218 const nav_msgs::msg::Path & path )
{
std::vector<PathSegment> segments;
PathSegment curr_segment;
curr_segment.start = 0;
// Iterating through the path to determine the position of the cusp
for ( unsigned int idx = 1; idx < path.poses.size( ) - 1; ++idx ) {
// We have two vectors for the dot product OA and AB. Determining the vectors.
double oa_x = path.poses[idx].pose.position.x -
path.poses[idx - 1].pose.position.x;
double oa_y = path.poses[idx].pose.position.y -
path.poses[idx - 1].pose.position.y;
double ab_x = path.poses[idx + 1].pose.position.x -
path.poses[idx].pose.position.x;
double ab_y = path.poses[idx + 1].pose.position.y -
path.poses[idx].pose.position.y;
// Checking for the existance of cusp, in the path, using the dot product.
double dot_product = ( oa_x * ab_x ) + ( oa_y * ab_y );
if ( dot_product < 0.0 ) {
curr_segment.end = idx;
segments.push_back( curr_segment );
curr_segment.start = idx;
}
// Checking for the existance of a differential rotation in place.
double cur_theta = tf2::getYaw( path.poses[idx].pose.orientation );
double next_theta = tf2::getYaw( path.poses[idx + 1].pose.orientation );
double dtheta = angles::shortest_angular_distance( cur_theta, next_theta );
if ( fabs( ab_x ) < 1e-4 && fabs( ab_y ) < 1e-4 && fabs( dtheta ) > 1e-4 ) {
curr_segment.end = idx;
segments.push_back( curr_segment );
curr_segment.start = idx;
}
}
curr_segment.end = path.poses.size( ) - 1;
segments.push_back( curr_segment );
return segments;
}
260 void SimpleSmoother::updateApproximatePathOrientations(
261 nav_msgs::msg::Path & path,
262 bool & reversing_segment )
{
double dx, dy, theta, pt_yaw;
reversing_segment = false;
// Find if this path segment is in reverse
dx = path.poses[2].pose.position.x - path.poses[1].pose.position.x;
dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y;
theta = atan2( dy, dx );
pt_yaw = tf2::getYaw( path.poses[1].pose.orientation );
if ( fabs( angles::shortest_angular_distance( pt_yaw, theta ) ) > M_PI_2 ) {
reversing_segment = true;
}
// Find the angle relative the path position vectors
for ( unsigned int i = 0; i != path.poses.size( ) - 1; i++ ) {
dx = path.poses[i + 1].pose.position.x - path.poses[i].pose.position.x;
dy = path.poses[i + 1].pose.position.y - path.poses[i].pose.position.y;
theta = atan2( dy, dx );
// If points are overlapping, pass
if ( fabs( dx ) < 1e-4 && fabs( dy ) < 1e-4 ) {
continue;
}
// Flip the angle if this path segment is in reverse
if ( reversing_segment ) {
theta += M_PI; // orientationAroundZAxis will normalize
}
path.poses[i].pose.orientation = orientationAroundZAxis( theta );
}
}
} // namespace nav2_smoother
#include "pluginlib/class_list_macros.hpp"
299 PLUGINLIB_EXPORT_CLASS( nav2_smoother::SimpleSmoother, nav2_core::Smoother )
1 // Copyright ( c ) 2022, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <math.h>
#include <memory>
#include <string>
#include <vector>
#include <chrono>
#include <limits>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav2_costmap_2d/costmap_subscriber.hpp"
#include "nav2_msgs/msg/costmap.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_smoother/simple_smoother.hpp"
#include "ament_index_cpp/get_package_share_directory.hpp"
using namespace nav2_smoother; // NOLINT
using namespace std::chrono_literals; // NOLINT
34 class RclCppFixture
{
public:
37 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
38 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
42 class SmootherWrapper : public nav2_smoother::SimpleSmoother
{
public:
45 SmootherWrapper( )
: nav2_smoother::SimpleSmoother( )
{
}
50 std::vector<PathSegment> findDirectionalPathSegmentsWrapper( nav_msgs::msg::Path path )
{
return findDirectionalPathSegments( path );
}
55 void setMaxItsToInvalid( )
{
max_its_ = 0;
}
};
61 TEST( SmootherTest, test_simple_smoother )
{
rclcpp_lifecycle::LifecycleNode::SharedPtr node =
std::make_shared<rclcpp_lifecycle::LifecycleNode>( "SmacSmootherTest" );
std::shared_ptr<nav2_msgs::msg::Costmap> costmap_msg =
std::make_shared<nav2_msgs::msg::Costmap>( );
costmap_msg->header.stamp = node->now( );
costmap_msg->header.frame_id = "map";
costmap_msg->data.resize( 100 * 100 );
costmap_msg->metadata.resolution = 0.05;
costmap_msg->metadata.size_x = 100;
costmap_msg->metadata.size_y = 100;
// island in the middle of lethal cost to cross
for ( unsigned int i = 20; i <= 30; ++i ) {
for ( unsigned int j = 20; j <= 30; ++j ) {
costmap_msg->data[j * 100 + i] = 254;
}
}
std::weak_ptr<rclcpp_lifecycle::LifecycleNode> parent = node;
std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> dummy_costmap;
dummy_costmap = std::make_shared<nav2_costmap_2d::CostmapSubscriber>( parent, "dummy_topic" );
dummy_costmap->costmapCallback( costmap_msg );
// Make smoother
std::shared_ptr<tf2_ros::Buffer> dummy_tf;
std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> dummy_footprint;
auto smoother = std::make_unique<SmootherWrapper>( );
smoother->configure( parent, "test", dummy_tf, dummy_costmap, dummy_footprint );
// Test that an irregular distributed path becomes more distributed
nav_msgs::msg::Path straight_irregular_path;
straight_irregular_path.header.frame_id = "map";
straight_irregular_path.header.stamp = node->now( );
straight_irregular_path.poses.resize( 11 );
straight_irregular_path.poses[0].pose.position.x = 0.5;
straight_irregular_path.poses[0].pose.position.y = 0.0;
straight_irregular_path.poses[1].pose.position.x = 0.5;
straight_irregular_path.poses[1].pose.position.y = 0.1;
straight_irregular_path.poses[2].pose.position.x = 0.5;
straight_irregular_path.poses[2].pose.position.y = 0.2;
straight_irregular_path.poses[3].pose.position.x = 0.5;
straight_irregular_path.poses[3].pose.position.y = 0.35;
straight_irregular_path.poses[4].pose.position.x = 0.5;
straight_irregular_path.poses[4].pose.position.y = 0.4;
straight_irregular_path.poses[5].pose.position.x = 0.5;
straight_irregular_path.poses[5].pose.position.y = 0.56;
straight_irregular_path.poses[6].pose.position.x = 0.5;
straight_irregular_path.poses[6].pose.position.y = 0.9;
straight_irregular_path.poses[7].pose.position.x = 0.5;
straight_irregular_path.poses[7].pose.position.y = 0.95;
straight_irregular_path.poses[8].pose.position.x = 0.5;
straight_irregular_path.poses[8].pose.position.y = 1.3;
straight_irregular_path.poses[9].pose.position.x = 0.5;
straight_irregular_path.poses[9].pose.position.y = 2.0;
straight_irregular_path.poses[10].pose.position.x = 0.5;
straight_irregular_path.poses[10].pose.position.y = 2.5;
rclcpp::Duration no_time = rclcpp::Duration::from_seconds( 0.0 ); // 0 seconds
rclcpp::Duration max_time = rclcpp::Duration::from_seconds( 1 ); // 1 second
EXPECT_FALSE( smoother->smooth( straight_irregular_path, no_time ) );
EXPECT_TRUE( smoother->smooth( straight_irregular_path, max_time ) );
for ( uint i = 0; i != straight_irregular_path.poses.size( ) - 1; i++ ) {
// Check distances are more evenly spaced out now
EXPECT_LT(
fabs(
straight_irregular_path.poses[i].pose.position.y -
straight_irregular_path.poses[i + 1].pose.position.y ), 0.38 );
}
// Test regular path, should see no effective change
nav_msgs::msg::Path straight_regular_path;
straight_regular_path.header = straight_irregular_path.header;
straight_regular_path.poses.resize( 11 );
straight_regular_path.poses[0].pose.position.x = 0.5;
straight_regular_path.poses[0].pose.position.y = 0.0;
straight_regular_path.poses[1].pose.position.x = 0.5;
straight_regular_path.poses[1].pose.position.y = 0.1;
straight_regular_path.poses[2].pose.position.x = 0.5;
straight_regular_path.poses[2].pose.position.y = 0.2;
straight_regular_path.poses[3].pose.position.x = 0.5;
straight_regular_path.poses[3].pose.position.y = 0.3;
straight_regular_path.poses[4].pose.position.x = 0.5;
straight_regular_path.poses[4].pose.position.y = 0.4;
straight_regular_path.poses[5].pose.position.x = 0.5;
straight_regular_path.poses[5].pose.position.y = 0.5;
straight_regular_path.poses[6].pose.position.x = 0.5;
straight_regular_path.poses[6].pose.position.y = 0.6;
straight_regular_path.poses[7].pose.position.x = 0.5;
straight_regular_path.poses[7].pose.position.y = 0.7;
straight_regular_path.poses[8].pose.position.x = 0.5;
straight_regular_path.poses[8].pose.position.y = 0.8;
straight_regular_path.poses[9].pose.position.x = 0.5;
straight_regular_path.poses[9].pose.position.y = 0.9;
straight_regular_path.poses[10].pose.position.x = 0.5;
straight_regular_path.poses[10].pose.position.y = 1.0;
EXPECT_TRUE( smoother->smooth( straight_regular_path, max_time ) );
for ( uint i = 0; i != straight_regular_path.poses.size( ) - 1; i++ ) {
// Check distances are still very evenly spaced
EXPECT_NEAR(
fabs(
straight_regular_path.poses[i].pose.position.y -
straight_regular_path.poses[i + 1].pose.position.y ), 0.1, 0.001 );
}
// test shorter and curved if given a right angle
nav_msgs::msg::Path right_angle_path;
right_angle_path = straight_regular_path;
straight_regular_path.poses[6].pose.position.x = 0.6;
straight_regular_path.poses[6].pose.position.y = 0.5;
straight_regular_path.poses[7].pose.position.x = 0.7;
straight_regular_path.poses[7].pose.position.y = 0.5;
straight_regular_path.poses[8].pose.position.x = 0.8;
straight_regular_path.poses[8].pose.position.y = 0.5;
straight_regular_path.poses[9].pose.position.x = 0.9;
straight_regular_path.poses[9].pose.position.y = 0.5;
straight_regular_path.poses[10].pose.position.x = 0.95;
straight_regular_path.poses[10].pose.position.y = 0.5;
EXPECT_TRUE( smoother->smooth( straight_regular_path, max_time ) );
EXPECT_NEAR( straight_regular_path.poses[5].pose.position.x, 0.637, 0.01 );
EXPECT_NEAR( straight_regular_path.poses[5].pose.position.y, 0.353, 0.01 );
// Test that collisions are rejected
nav_msgs::msg::Path collision_path;
collision_path.poses.resize( 11 );
collision_path.poses[0].pose.position.x = 0.0;
collision_path.poses[0].pose.position.y = 0.0;
collision_path.poses[1].pose.position.x = 0.2;
collision_path.poses[1].pose.position.y = 0.2;
collision_path.poses[2].pose.position.x = 0.4;
collision_path.poses[2].pose.position.y = 0.4;
collision_path.poses[3].pose.position.x = 0.6;
collision_path.poses[3].pose.position.y = 0.6;
collision_path.poses[4].pose.position.x = 0.8;
collision_path.poses[4].pose.position.y = 0.8;
collision_path.poses[5].pose.position.x = 1.0;
collision_path.poses[5].pose.position.y = 1.0;
collision_path.poses[6].pose.position.x = 1.1;
collision_path.poses[6].pose.position.y = 1.1;
collision_path.poses[7].pose.position.x = 1.2;
collision_path.poses[7].pose.position.y = 1.2;
collision_path.poses[8].pose.position.x = 1.3;
collision_path.poses[8].pose.position.y = 1.3;
collision_path.poses[9].pose.position.x = 1.4;
collision_path.poses[9].pose.position.y = 1.4;
collision_path.poses[10].pose.position.x = 1.5;
collision_path.poses[10].pose.position.y = 1.5;
EXPECT_FALSE( smoother->smooth( collision_path, max_time ) );
// test cusp / reversing segments
nav_msgs::msg::Path reversing_path;
reversing_path.poses.resize( 11 );
reversing_path.poses[0].pose.position.x = 0.5;
reversing_path.poses[0].pose.position.y = 0.0;
reversing_path.poses[1].pose.position.x = 0.5;
reversing_path.poses[1].pose.position.y = 0.1;
reversing_path.poses[2].pose.position.x = 0.5;
reversing_path.poses[2].pose.position.y = 0.2;
reversing_path.poses[3].pose.position.x = 0.5;
reversing_path.poses[3].pose.position.y = 0.3;
reversing_path.poses[4].pose.position.x = 0.5;
reversing_path.poses[4].pose.position.y = 0.4;
reversing_path.poses[5].pose.position.x = 0.5;
reversing_path.poses[5].pose.position.y = 0.5;
reversing_path.poses[6].pose.position.x = 0.5;
reversing_path.poses[6].pose.position.y = 0.4;
reversing_path.poses[7].pose.position.x = 0.5;
reversing_path.poses[7].pose.position.y = 0.3;
reversing_path.poses[8].pose.position.x = 0.5;
reversing_path.poses[8].pose.position.y = 0.2;
reversing_path.poses[9].pose.position.x = 0.5;
reversing_path.poses[9].pose.position.y = 0.1;
reversing_path.poses[10].pose.position.x = 0.5;
reversing_path.poses[10].pose.position.y = 0.0;
EXPECT_TRUE( smoother->smooth( reversing_path, max_time ) );
// // test rotate in place
tf2::Quaternion quat1, quat2;
quat1.setRPY( 0.0, 0.0, 0.0 );
quat2.setRPY( 0.0, 0.0, 1.0 );
straight_irregular_path.poses[5].pose.position.x = 0.5;
straight_irregular_path.poses[5].pose.position.y = 0.5;
straight_irregular_path.poses[5].pose.orientation = tf2::toMsg( quat1 );
straight_irregular_path.poses[6].pose.position.x = 0.5;
straight_irregular_path.poses[6].pose.position.y = 0.5;
straight_irregular_path.poses[6].pose.orientation = tf2::toMsg( quat2 );
EXPECT_TRUE( smoother->smooth( straight_irregular_path, max_time ) );
// test max iterations
smoother->setMaxItsToInvalid( );
nav_msgs::msg::Path max_its_path;
max_its_path.poses.resize( 11 );
max_its_path.poses[0].pose.position.x = 0.5;
max_its_path.poses[0].pose.position.y = 0.0;
max_its_path.poses[1].pose.position.x = 0.5;
max_its_path.poses[1].pose.position.y = 0.1;
max_its_path.poses[2].pose.position.x = 0.5;
max_its_path.poses[2].pose.position.y = 0.2;
max_its_path.poses[3].pose.position.x = 0.5;
max_its_path.poses[3].pose.position.y = 0.3;
max_its_path.poses[4].pose.position.x = 0.5;
max_its_path.poses[4].pose.position.y = 0.4;
max_its_path.poses[5].pose.position.x = 0.5;
max_its_path.poses[5].pose.position.y = 0.5;
max_its_path.poses[6].pose.position.x = 0.5;
max_its_path.poses[6].pose.position.y = 0.6;
max_its_path.poses[7].pose.position.x = 0.5;
max_its_path.poses[7].pose.position.y = 0.7;
max_its_path.poses[8].pose.position.x = 0.5;
max_its_path.poses[8].pose.position.y = 0.8;
max_its_path.poses[9].pose.position.x = 0.5;
max_its_path.poses[9].pose.position.y = 0.9;
max_its_path.poses[10].pose.position.x = 0.5;
max_its_path.poses[10].pose.position.y = 1.0;
EXPECT_FALSE( smoother->smooth( max_its_path, max_time ) );
}
// Copyright ( c ) 2021 RoboTech Vision
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <string>
#include <memory>
#include <chrono>
#include <iostream>
#include <future>
#include <thread>
#include <algorithm>
#include <vector>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_core/smoother.hpp"
#include "nav2_core/exceptions.hpp"
#include "nav2_msgs/action/smooth_path.hpp"
#include "nav2_smoother/nav2_smoother.hpp"
#include "tf2_ros/create_timer_ros.h"
using SmoothAction = nav2_msgs::action::SmoothPath;
using ClientGoalHandle = rclcpp_action::ClientGoalHandle<SmoothAction>;
using namespace std::chrono_literals;
// A smoother for testing the base class
41 class DummySmoother : public nav2_core::Smoother
{
public:
44 DummySmoother( )
: initialized_( false ) {}
47 ~DummySmoother( ) {}
49 virtual void configure(
50 const rclcpp_lifecycle::LifecycleNode::WeakPtr &,
51 std::string, std::shared_ptr<tf2_ros::Buffer>,
52 std::shared_ptr<nav2_costmap_2d::CostmapSubscriber>,
53 std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> ) {}
55 virtual void cleanup( ) {}
57 virtual void activate( ) {}
59 virtual void deactivate( ) {}
61 virtual bool smooth(
62 nav_msgs::msg::Path & path,
63 const rclcpp::Duration & max_time )
{
assert( path.poses.size( ) == 2 );
if ( path.poses.front( ) == path.poses.back( ) ) {
throw nav2_core::PlannerException( "Start and goal pose must differ" );
}
auto max_time_ms = max_time.to_chrono<std::chrono::milliseconds>( );
std::this_thread::sleep_for( std::min( max_time_ms, 100ms ) );
// place dummy pose in the middle of the path
geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x =
( path.poses.front( ).pose.position.x + path.poses.back( ).pose.position.x ) / 2;
pose.pose.position.y =
( path.poses.front( ).pose.position.y + path.poses.back( ).pose.position.y ) / 2;
pose.pose.orientation.w = 1.0;
path.poses.push_back( pose );
return max_time_ms > 100ms;
}
private:
87 bool initialized_;
88 std::string command_;
89 std::chrono::system_clock::time_point start_time_;
};
// Mocked class loader
93 void onPluginDeletion( nav2_core::Smoother * obj )
{
if ( nullptr != obj ) {
delete ( obj );
}
}
template<>
101 pluginlib::UniquePtr<nav2_core::Smoother> pluginlib::ClassLoader<nav2_core::Smoother>::
102 createUniqueInstance( const std::string & lookup_name )
{
if ( lookup_name != "DummySmoother" ) {
// original method body
if ( !isClassLoaded( lookup_name ) ) {
loadLibraryForClass( lookup_name );
}
try {
std::string class_type = getClassType( lookup_name );
pluginlib::UniquePtr<nav2_core::Smoother> obj =
lowlevel_class_loader_.createUniqueInstance<nav2_core::Smoother>( class_type );
return obj;
} catch ( const class_loader::CreateClassException & ex ) {
throw pluginlib::CreateClassException( ex.what( ) );
}
}
// mocked plugin creation
return std::unique_ptr<nav2_core::Smoother,
class_loader::ClassLoader::DeleterType<nav2_core::Smoother>>(
new DummySmoother( ),
onPluginDeletion );
}
126 class DummyCostmapSubscriber : public nav2_costmap_2d::CostmapSubscriber
{
public:
129 DummyCostmapSubscriber(
130 nav2_util::LifecycleNode::SharedPtr node,
131 const std::string & topic_name )
: CostmapSubscriber( node, topic_name )
{
auto costmap = std::make_shared<nav2_msgs::msg::Costmap>( );
costmap->metadata.size_x = 100;
costmap->metadata.size_y = 100;
costmap->metadata.resolution = 0.1;
costmap->metadata.origin.position.x = -5.0;
costmap->metadata.origin.position.y = -5.0;
costmap->data.resize( costmap->metadata.size_x * costmap->metadata.size_y, 0 );
for ( unsigned int i = 0; i < costmap->metadata.size_y; ++i ) {
for ( unsigned int j = 20; j < 40; ++j ) {
costmap->data[i * costmap->metadata.size_x + j] = 254;
}
}
setCostmap( costmap );
}
151 void setCostmap( nav2_msgs::msg::Costmap::SharedPtr msg )
{
costmap_msg_ = msg;
costmap_received_ = true;
}
};
158 class DummyFootprintSubscriber : public nav2_costmap_2d::FootprintSubscriber
{
public:
161 DummyFootprintSubscriber(
162 nav2_util::LifecycleNode::SharedPtr node,
163 const std::string & topic_name,
164 tf2_ros::Buffer & tf_ )
: FootprintSubscriber( node, topic_name, tf_ )
{
auto footprint = std::make_shared<geometry_msgs::msg::PolygonStamped>( );
footprint->header.frame_id = "base_link"; // global frame = robot frame to avoid tf lookup
footprint->header.stamp = node->get_clock( )->now( );
geometry_msgs::msg::Point32 point;
point.x = -0.2f;
point.y = -0.2f;
footprint->polygon.points.push_back( point );
point.y = 0.2f;
footprint->polygon.points.push_back( point );
point.x = 0.2f;
point.y = 0.0f;
footprint->polygon.points.push_back( point );
setFootprint( footprint );
}
183 void setFootprint( geometry_msgs::msg::PolygonStamped::SharedPtr msg )
{
footprint_ = msg;
footprint_received_ = true;
}
};
190 class DummySmootherServer : public nav2_smoother::SmootherServer
{
public:
193 DummySmootherServer( )
{
// Override defaults
default_ids_.clear( );
default_ids_.resize( 1, "SmoothPath" );
set_parameter( rclcpp::Parameter( "smoother_plugins", default_ids_ ) );
default_types_.clear( );
default_types_.resize( 1, "DummySmoother" );
}
nav2_util::CallbackReturn
204 on_configure( const rclcpp_lifecycle::State & state )
{
auto result = SmootherServer::on_configure( state );
if ( result != nav2_util::CallbackReturn::SUCCESS ) {
return result;
}
// Create dummy subscribers and collision checker
auto node = shared_from_this( );
costmap_sub_ =
std::make_shared<DummyCostmapSubscriber>(
node, "costmap_topic" );
footprint_sub_ =
std::make_shared<DummyFootprintSubscriber>(
node, "footprint_topic", *tf_ );
collision_checker_ =
std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(
*costmap_sub_, *footprint_sub_,
node->get_name( ) );
return result;
}
};
// Define a test class to hold the context for the tests
230 class SmootherConfigTest : public ::testing::Test
{
};
234 class SmootherTest : public ::testing::Test
{
protected:
237 SmootherTest( ) {SetUp( );}
238 ~SmootherTest( ) {}
240 void SetUp( )
{
node_lifecycle_ =
std::make_shared<rclcpp_lifecycle::LifecycleNode>(
"LifecycleSmootherTestNode", rclcpp::NodeOptions( ) );
smoother_server_ = std::make_shared<DummySmootherServer>( );
smoother_server_->set_parameter(
rclcpp::Parameter(
"smoother_plugins",
rclcpp::ParameterValue( std::vector<std::string>( 1, "DummySmoothPath" ) ) ) );
smoother_server_->declare_parameter(
"DummySmoothPath.plugin",
rclcpp::ParameterValue( std::string( "DummySmoother" ) ) );
smoother_server_->configure( );
smoother_server_->activate( );
client_ = rclcpp_action::create_client<SmoothAction>(
node_lifecycle_->get_node_base_interface( ),
node_lifecycle_->get_node_graph_interface( ),
node_lifecycle_->get_node_logging_interface( ),
node_lifecycle_->get_node_waitables_interface( ), "smooth_path" );
std::cout << "Setup complete." << std::endl;
}
void TearDown( ) override
{
smoother_server_->deactivate( );
smoother_server_->cleanup( );
smoother_server_->shutdown( );
}
bool sendGoal(
std::string smoother_id, double x_start, double y_start, double x_goal,
double y_goal, std::chrono::milliseconds max_time, bool check_for_collisions )
{
if ( !client_->wait_for_action_server( 4s ) ) {
std::cout << "Server not up" << std::endl;
return false;
}
geometry_msgs::msg::PoseStamped pose;
pose.pose.orientation.w = 1.0;
auto goal = SmoothAction::Goal( );
goal.smoother_id = smoother_id;
pose.pose.position.x = x_start;
pose.pose.position.y = y_start;
goal.path.poses.push_back( pose );
pose.pose.position.x = x_goal;
pose.pose.position.y = y_goal;
goal.path.poses.push_back( pose );
goal.check_for_collisions = check_for_collisions;
goal.max_smoothing_duration = rclcpp::Duration( max_time );
auto future_goal = client_->async_send_goal( goal );
if ( rclcpp::spin_until_future_complete( node_lifecycle_, future_goal ) !=
rclcpp::FutureReturnCode::SUCCESS )
{
std::cout << "failed sending goal" << std::endl;
// failed sending the goal
return false;
}
goal_handle_ = future_goal.get( );
if ( !goal_handle_ ) {
std::cout << "goal was rejected" << std::endl;
// goal was rejected by the action server
return false;
}
return true;
}
ClientGoalHandle::WrappedResult getResult( )
{
std::cout << "Getting async result..." << std::endl;
auto future_result = client_->async_get_result( goal_handle_ );
std::cout << "Waiting on future..." << std::endl;
rclcpp::spin_until_future_complete( node_lifecycle_, future_result );
std::cout << "future received!" << std::endl;
return future_result.get( );
}
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_lifecycle_;
std::shared_ptr<DummySmootherServer> smoother_server_;
std::shared_ptr<rclcpp_action::Client<SmoothAction>> client_;
std::shared_ptr<rclcpp_action::ClientGoalHandle<SmoothAction>> goal_handle_;
};
// Define the tests
TEST_F( SmootherTest, testingSuccess )
{
ASSERT_TRUE( sendGoal( "DummySmoothPath", 0.0, 0.0, 1.0, 0.0, 500ms, true ) );
auto result = getResult( );
EXPECT_EQ( result.code, rclcpp_action::ResultCode::SUCCEEDED );
EXPECT_EQ( result.result->path.poses.size( ), ( std::size_t )3 );
EXPECT_TRUE( result.result->was_completed );
SUCCEED( );
}
TEST_F( SmootherTest, testingFailureOnInvalidSmootherId )
{
ASSERT_TRUE( sendGoal( "InvalidSmoother", 0.0, 0.0, 1.0, 0.0, 500ms, true ) );
auto result = getResult( );
EXPECT_EQ( result.code, rclcpp_action::ResultCode::ABORTED );
SUCCEED( );
}
TEST_F( SmootherTest, testingSuccessOnEmptyPlugin )
{
ASSERT_TRUE( sendGoal( "", 0.0, 0.0, 1.0, 0.0, 500ms, true ) );
auto result = getResult( );
EXPECT_EQ( result.code, rclcpp_action::ResultCode::SUCCEEDED );
SUCCEED( );
}
TEST_F( SmootherTest, testingIncomplete )
{
ASSERT_TRUE( sendGoal( "DummySmoothPath", 0.0, 0.0, 1.0, 0.0, 50ms, true ) );
auto result = getResult( );
EXPECT_EQ( result.code, rclcpp_action::ResultCode::SUCCEEDED );
EXPECT_FALSE( result.result->was_completed );
SUCCEED( );
}
TEST_F( SmootherTest, testingFailureOnException )
{
ASSERT_TRUE( sendGoal( "DummySmoothPath", 0.0, 0.0, 0.0, 0.0, 500ms, true ) );
auto result = getResult( );
EXPECT_EQ( result.code, rclcpp_action::ResultCode::ABORTED );
SUCCEED( );
}
TEST_F( SmootherTest, testingFailureOnCollision )
{
ASSERT_TRUE( sendGoal( "DummySmoothPath", -4.0, 0.0, 0.0, 0.0, 500ms, true ) );
auto result = getResult( );
EXPECT_EQ( result.code, rclcpp_action::ResultCode::ABORTED );
SUCCEED( );
}
TEST_F( SmootherTest, testingCollisionCheckDisabled )
{
ASSERT_TRUE( sendGoal( "DummySmoothPath", -4.0, 0.0, 0.0, 0.0, 500ms, false ) );
auto result = getResult( );
EXPECT_EQ( result.code, rclcpp_action::ResultCode::SUCCEEDED );
SUCCEED( );
}
TEST_F( SmootherConfigTest, testingConfigureSuccessWithValidSmootherPlugin )
{
auto smoother_server = std::make_shared<DummySmootherServer>( );
smoother_server->set_parameter(
rclcpp::Parameter(
"smoother_plugins",
rclcpp::ParameterValue( std::vector<std::string>( 1, "DummySmoothPath" ) ) ) );
smoother_server->declare_parameter(
"DummySmoothPath.plugin",
rclcpp::ParameterValue( std::string( "DummySmoother" ) ) );
auto state = smoother_server->configure( );
EXPECT_EQ( state.id( ), 2 ); // 1 on failure, 2 on success
SUCCEED( );
}
TEST_F( SmootherConfigTest, testingConfigureFailureWithInvalidSmootherPlugin )
{
auto smoother_server = std::make_shared<DummySmootherServer>( );
smoother_server->set_parameter(
rclcpp::Parameter(
"smoother_plugins",
rclcpp::ParameterValue( std::vector<std::string>( 1, "DummySmoothPath" ) ) ) );
smoother_server->declare_parameter(
"DummySmoothPath.plugin",
rclcpp::ParameterValue( std::string( "InvalidSmootherPlugin" ) ) );
auto state = smoother_server->configure( );
EXPECT_EQ( state.id( ), 1 ); // 1 on failure, 2 on success
SUCCEED( );
}
TEST_F( SmootherConfigTest, testingConfigureSuccessWithDefaultPlugin )
{
auto smoother_server = std::make_shared<DummySmootherServer>( );
auto state = smoother_server->configure( );
EXPECT_EQ( state.id( ), 2 ); // 1 on failure, 2 on success
SUCCEED( );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef BEHAVIOR_TREE__DUMMY_SERVERS_HPP_
#define BEHAVIOR_TREE__DUMMY_SERVERS_HPP_
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include <chrono>
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp/rclcpp.hpp"
using namespace std::chrono_literals; // NOLINT
using namespace std::chrono; // NOLINT
using namespace std::placeholders; // NOLINT
template<class ServiceT>
32 class DummyService
{
public:
35 explicit DummyService(
36 const rclcpp::Node::SharedPtr & node,
37 std::string service_name )
: node_( node ),
service_name_( service_name ),
request_count_( 0 ),
disabled_( false )
{
server_ = node->create_service<ServiceT>(
service_name,
std::bind( &DummyService::handle_service, this, _1, _2, _3 ) );
}
48 void disable( )
{
server_.reset( );
disabled_ = true;
}
54 void enable( )
{
if ( disabled_ ) {
server_ = node_->create_service<ServiceT>(
service_name_,
std::bind( &DummyService::handle_service, this, _1, _2, _3 ) );
disabled_ = false;
}
}
64 void reset( )
{
enable( );
request_count_ = 0;
}
70 int getRequestCount( ) const
{
return request_count_;
}
protected:
76 virtual void fillResponse(
77 const std::shared_ptr<typename ServiceT::Request>/*request*/,
78 const std::shared_ptr<typename ServiceT::Response>/*response*/ ) {}
80 void handle_service(
81 const std::shared_ptr<rmw_request_id_t>/*request_header*/,
82 const std::shared_ptr<typename ServiceT::Request> request,
83 const std::shared_ptr<typename ServiceT::Response> response )
{
request_count_++;
fillResponse( request, response );
}
private:
90 rclcpp::Node::SharedPtr node_;
typename rclcpp::Service<ServiceT>::SharedPtr server_;
std::string service_name_;
int request_count_;
bool disabled_;
};
template<class ActionT>
98 class DummyActionServer
{
public:
101 explicit DummyActionServer(
102 const rclcpp::Node::SharedPtr & node,
103 std::string action_name )
: action_name_( action_name ),
goal_count_( 0 )
{
this->action_server_ = rclcpp_action::create_server<ActionT>(
node->get_node_base_interface( ),
node->get_node_clock_interface( ),
node->get_node_logging_interface( ),
node->get_node_waitables_interface( ),
action_name,
std::bind( &DummyActionServer::handle_goal, this, _1, _2 ),
std::bind( &DummyActionServer::handle_cancel, this, _1 ),
std::bind( &DummyActionServer::handle_accepted, this, _1 ) );
}
118 void setFailureRanges( const std::vector<std::pair<int, int>> & failureRanges )
{
failure_ranges_ = failureRanges;
}
123 void setRunningRanges( const std::vector<std::pair<int, int>> & runningRanges )
{
running_ranges_ = runningRanges;
}
128 void reset( )
{
failure_ranges_.clear( );
running_ranges_.clear( );
goal_count_ = 0;
}
135 int getGoalCount( ) const
{
return goal_count_;
}
protected:
141 virtual std::shared_ptr<typename ActionT::Result> fillResult( )
{
return std::make_shared<typename ActionT::Result>( );
}
146 virtual rclcpp_action::GoalResponse handle_goal(
147 const rclcpp_action::GoalUUID &,
148 std::shared_ptr<const typename ActionT::Goal>/*goal*/ )
{
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
153 virtual rclcpp_action::CancelResponse handle_cancel(
const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> )
{
return rclcpp_action::CancelResponse::ACCEPT;
}
159 void execute(
const typename std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> goal_handle )
{
goal_count_++;
auto result = fillResult( );
// if current goal index exists in running range, the thread sleeps for 1 second
// to simulate a long running action
for ( auto & index : running_ranges_ ) {
if ( goal_count_ >= index.first && goal_count_ <= index.second ) {
std::this_thread::sleep_for( 1s );
break;
}
}
// if current goal index exists in failure range, the goal will be aborted
for ( auto & index : failure_ranges_ ) {
if ( goal_count_ >= index.first && goal_count_ <= index.second ) {
goal_handle->abort( result );
return;
}
}
// goal succeeds for all other indices
goal_handle->succeed( result );
}
186 void handle_accepted(
const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> goal_handle )
{
using namespace std::placeholders; // NOLINT
// this needs to return quickly to avoid blocking the executor, so spin up a new thread
std::thread{std::bind( &DummyActionServer::execute, this, _1 ), goal_handle}.detach( );
}
protected:
typename rclcpp_action::Server<ActionT>::SharedPtr action_server_;
std::string action_name_;
// contains pairs of indices which define a range for which the
// requested action goal will return running for 1s or be aborted
// for all other indices, the action server will return success
std::vector<std::pair<int, int>> failure_ranges_;
std::vector<std::pair<int, int>> running_ranges_;
int goal_count_;
};
#endif // BEHAVIOR_TREE__DUMMY_SERVERS_HPP_
1 // Copyright ( c ) 2020 Vinny Ruia
// Copyright ( c ) 2020 Sarthak Mittal
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <memory>
#include <thread>
#include "server_handler.hpp"
using namespace std::chrono_literals; // NOLINT
using namespace std::chrono; // NOLINT
25 ServerHandler::ServerHandler( )
: is_active_( false )
{
node_ = rclcpp::Node::make_shared( "behavior_tree_tester" );
clear_local_costmap_server = std::make_unique<DummyService<nav2_msgs::srv::ClearEntireCostmap>>(
node_, "local_costmap/clear_entirely_local_costmap" );
clear_global_costmap_server = std::make_unique<DummyService<nav2_msgs::srv::ClearEntireCostmap>>(
node_, "global_costmap/clear_entirely_global_costmap" );
compute_path_to_pose_server = std::make_unique<ComputePathToPoseActionServer>( node_ );
follow_path_server = std::make_unique<DummyActionServer<nav2_msgs::action::FollowPath>>(
node_, "follow_path" );
spin_server = std::make_unique<DummyActionServer<nav2_msgs::action::Spin>>(
node_, "spin" );
wait_server = std::make_unique<DummyActionServer<nav2_msgs::action::Wait>>(
node_, "wait" );
backup_server = std::make_unique<DummyActionServer<nav2_msgs::action::BackUp>>(
node_, "backup" );
drive_on_heading_server = std::make_unique<DummyActionServer<nav2_msgs::action::DriveOnHeading>>(
node_, "drive_on_heading" );
ntp_server = std::make_unique<DummyActionServer<nav2_msgs::action::ComputePathThroughPoses>>(
node_, "compute_path_through_poses" );
}
49 ServerHandler::~ServerHandler( )
{
if ( is_active_ ) {
deactivate( );
}
}
56 void ServerHandler::activate( )
{
if ( is_active_ ) {
throw std::runtime_error( "Trying to activate while already activated" );
}
is_active_ = true;
server_thread_ =
std::make_shared<std::thread>( std::bind( &ServerHandler::spinThread, this ) );
std::cout << "Server handler is active!" << std::endl;
}
69 void ServerHandler::deactivate( )
{
if ( !is_active_ ) {
throw std::runtime_error( "Trying to deactivate while already inactive" );
}
is_active_ = false;
server_thread_->join( );
std::cout << "Server handler has been deactivated!" << std::endl;
}
81 void ServerHandler::reset( ) const
{
clear_global_costmap_server->reset( );
clear_local_costmap_server->reset( );
compute_path_to_pose_server->reset( );
follow_path_server->reset( );
spin_server->reset( );
wait_server->reset( );
backup_server->reset( );
drive_on_heading_server->reset( );
}
93 void ServerHandler::spinThread( )
{
rclcpp::spin( node_ );
}
// Copyright ( c ) 2020 Vinny Ruia
// Copyright ( c ) 2020 Sarthak Mittal
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef BEHAVIOR_TREE__SERVER_HANDLER_HPP_
#define BEHAVIOR_TREE__SERVER_HANDLER_HPP_
#include <memory>
#include <string>
#include <thread>
#include <utility>
#include <vector>
#include "nav2_msgs/srv/clear_entire_costmap.hpp"
#include "nav2_msgs/action/compute_path_to_pose.hpp"
#include "nav2_msgs/action/follow_path.hpp"
#include "nav2_msgs/action/spin.hpp"
#include "nav2_msgs/action/back_up.hpp"
#include "nav2_msgs/action/wait.hpp"
#include "nav2_msgs/action/drive_on_heading.hpp"
#include "nav2_msgs/action/compute_path_through_poses.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "dummy_servers.hpp"
41 class ComputePathToPoseActionServer
42 : public DummyActionServer<nav2_msgs::action::ComputePathToPose>
{
public:
45 explicit ComputePathToPoseActionServer( const rclcpp::Node::SharedPtr & node )
: DummyActionServer( node, "compute_path_to_pose" )
{
result_ = std::make_shared<nav2_msgs::action::ComputePathToPose::Result>( );
geometry_msgs::msg::PoseStamped pose;
pose.header = result_->path.header;
pose.pose.position.x = 0.0;
pose.pose.position.y = 0.0;
pose.pose.position.z = 0.0;
pose.pose.orientation.x = 0.0;
pose.pose.orientation.y = 0.0;
pose.pose.orientation.z = 0.0;
pose.pose.orientation.w = 1.0;
for ( int i = 0; i < 6; ++i ) {
result_->path.poses.push_back( pose );
}
}
std::shared_ptr<nav2_msgs::action::ComputePathToPose::Result> fillResult( ) override
{
return result_;
}
private:
std::shared_ptr<nav2_msgs::action::ComputePathToPose::Result> result_;
};
class ServerHandler
{
public:
ServerHandler( );
~ServerHandler( );
void activate( );
void deactivate( );
bool isActive( ) const
{
return is_active_;
}
void reset( ) const;
public:
std::unique_ptr<DummyService<nav2_msgs::srv::ClearEntireCostmap>> clear_local_costmap_server;
std::unique_ptr<DummyService<nav2_msgs::srv::ClearEntireCostmap>> clear_global_costmap_server;
std::unique_ptr<ComputePathToPoseActionServer> compute_path_to_pose_server;
std::unique_ptr<DummyActionServer<nav2_msgs::action::FollowPath>> follow_path_server;
std::unique_ptr<DummyActionServer<nav2_msgs::action::Spin>> spin_server;
std::unique_ptr<DummyActionServer<nav2_msgs::action::Wait>> wait_server;
std::unique_ptr<DummyActionServer<nav2_msgs::action::BackUp>> backup_server;
std::unique_ptr<DummyActionServer<nav2_msgs::action::DriveOnHeading>> drive_on_heading_server;
std::unique_ptr<DummyActionServer<nav2_msgs::action::ComputePathThroughPoses>> ntp_server;
private:
void spinThread( );
bool is_active_;
rclcpp::Node::SharedPtr node_;
std::shared_ptr<std::thread> server_thread_;
};
#endif // BEHAVIOR_TREE__SERVER_HANDLER_HPP_
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <vector>
#include <string>
#include <fstream>
#include <memory>
#include <utility>
#include <boost/filesystem.hpp>
#include "gtest/gtest.h"
#include "behaviortree_cpp_v3/behavior_tree.h"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behaviortree_cpp_v3/utils/shared_library.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/create_timer_ros.h"
#include "rclcpp/rclcpp.hpp"
#include "ament_index_cpp/get_package_share_directory.hpp"
#include "server_handler.hpp"
using namespace std::chrono_literals;
namespace fs = boost::filesystem;
40 class BehaviorTreeHandler
{
public:
43 BehaviorTreeHandler( )
{
node_ = rclcpp::Node::make_shared( "behavior_tree_handler" );
tf_ = std::make_shared<tf2_ros::Buffer>( node_->get_clock( ) );
auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
node_->get_node_base_interface( ), node_->get_node_timers_interface( ) );
tf_->setCreateTimerInterface( timer_interface );
tf_->setUsingDedicatedThread( true );
tf_listener_ = std::make_shared<tf2_ros::TransformListener>( *tf_, node_, false );
const std::vector<std::string> plugin_libs = {
"nav2_compute_path_to_pose_action_bt_node",
"nav2_compute_path_through_poses_action_bt_node",
"nav2_smooth_path_action_bt_node",
"nav2_follow_path_action_bt_node",
"nav2_spin_action_bt_node",
"nav2_wait_action_bt_node",
"nav2_back_up_action_bt_node",
"nav2_drive_on_heading_bt_node",
"nav2_clear_costmap_service_bt_node",
"nav2_is_stuck_condition_bt_node",
"nav2_goal_reached_condition_bt_node",
"nav2_initial_pose_received_condition_bt_node",
"nav2_goal_updated_condition_bt_node",
"nav2_globally_updated_goal_condition_bt_node",
"nav2_is_path_valid_condition_bt_node",
"nav2_reinitialize_global_localization_service_bt_node",
"nav2_rate_controller_bt_node",
"nav2_distance_controller_bt_node",
"nav2_speed_controller_bt_node",
"nav2_truncate_path_action_bt_node",
"nav2_truncate_path_local_action_bt_node",
"nav2_goal_updater_node_bt_node",
"nav2_recovery_node_bt_node",
"nav2_pipeline_sequence_bt_node",
"nav2_round_robin_node_bt_node",
"nav2_transform_available_condition_bt_node",
"nav2_time_expired_condition_bt_node",
"nav2_path_expiring_timer_condition",
"nav2_distance_traveled_condition_bt_node",
"nav2_single_trigger_bt_node",
"nav2_is_battery_low_condition_bt_node",
"nav2_navigate_through_poses_action_bt_node",
"nav2_navigate_to_pose_action_bt_node",
"nav2_remove_passed_goals_action_bt_node",
"nav2_planner_selector_bt_node",
"nav2_controller_selector_bt_node",
"nav2_goal_checker_selector_bt_node",
"nav2_controller_cancel_bt_node",
"nav2_path_longer_on_approach_bt_node",
"nav2_wait_cancel_bt_node",
"nav2_spin_cancel_bt_node",
"nav2_back_up_cancel_bt_node",
"nav2_drive_on_heading_cancel_bt_node"
};
for ( const auto & p : plugin_libs ) {
factory_.registerFromPlugin( BT::SharedLibrary::getOSName( p ) );
}
}
104 bool loadBehaviorTree( const std::string & filename )
{
// Read the input BT XML from the specified file into a string
std::ifstream xml_file( filename );
if ( !xml_file.good( ) ) {
RCLCPP_ERROR( node_->get_logger( ), "Couldn't open input XML file: %s", filename.c_str( ) );
return false;
}
auto xml_string = std::string(
std::istreambuf_iterator<char>( xml_file ),
std::istreambuf_iterator<char>( ) );
// Create the blackboard that will be shared by all of the nodes in the tree
blackboard = BT::Blackboard::create( );
// Put items on the blackboard
blackboard->set<rclcpp::Node::SharedPtr>( "node", node_ ); // NOLINT
blackboard->set<std::chrono::milliseconds>(
"server_timeout", std::chrono::milliseconds( 20 ) ); // NOLINT
blackboard->set<std::chrono::milliseconds>(
"bt_loop_duration", std::chrono::milliseconds( 10 ) ); // NOLINT
blackboard->set<std::shared_ptr<tf2_ros::Buffer>>( "tf_buffer", tf_ ); // NOLINT
blackboard->set<bool>( "initial_pose_received", false ); // NOLINT
blackboard->set<int>( "number_recoveries", 0 ); // NOLINT
// set dummy goal on blackboard
geometry_msgs::msg::PoseStamped goal;
goal.header.stamp = node_->now( );
goal.header.frame_id = "map";
goal.pose.position.x = 0.0;
goal.pose.position.y = 0.0;
goal.pose.position.z = 0.0;
goal.pose.orientation.x = 0.0;
goal.pose.orientation.y = 0.0;
goal.pose.orientation.z = 0.0;
goal.pose.orientation.w = 1.0;
blackboard->set<geometry_msgs::msg::PoseStamped>( "goal", goal ); // NOLINT
// Create the Behavior Tree from the XML input
try {
tree = factory_.createTreeFromText( xml_string, blackboard );
} catch ( BT::RuntimeError & exp ) {
RCLCPP_ERROR( node_->get_logger( ), "%s: %s", filename.c_str( ), exp.what( ) );
return false;
}
return true;
}
public:
157 BT::Blackboard::Ptr blackboard;
158 BT::Tree tree;
private:
161 rclcpp::Node::SharedPtr node_;
163 std::shared_ptr<tf2_ros::Buffer> tf_;
164 std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
166 BT::BehaviorTreeFactory factory_;
};
169 class BehaviorTreeTestFixture : public ::testing::Test
{
public:
172 static void SetUpTestCase( )
{
// initialize ROS
rclcpp::init( 0, nullptr );
server_handler = std::make_shared<ServerHandler>( );
if ( !server_handler->isActive( ) ) {
server_handler->activate( );
}
}
183 static void TearDownTestCase( )
{
// shutdown ROS
rclcpp::shutdown( );
server_handler.reset( );
bt_handler.reset( );
}
void SetUp( ) override
{
server_handler->reset( );
bt_handler = std::make_shared<BehaviorTreeHandler>( );
}
void TearDown( ) override
{
bt_handler.reset( );
}
protected:
static std::shared_ptr<ServerHandler> server_handler;
static std::shared_ptr<BehaviorTreeHandler> bt_handler;
};
std::shared_ptr<ServerHandler> BehaviorTreeTestFixture::server_handler = nullptr;
std::shared_ptr<BehaviorTreeHandler> BehaviorTreeTestFixture::bt_handler = nullptr;
TEST_F( BehaviorTreeTestFixture, TestBTXMLFiles )
{
fs::path root = ament_index_cpp::get_package_share_directory( "nav2_bt_navigator" );
root /= "behavior_trees/";
if ( boost::filesystem::exists( root ) && boost::filesystem::is_directory( root ) ) {
for ( auto const & entry : boost::filesystem::recursive_directory_iterator( root ) ) {
if ( boost::filesystem::is_regular_file( entry ) && entry.path( ).extension( ) == ".xml" ) {
EXPECT_EQ( bt_handler->loadBehaviorTree( entry.path( ).string( ) ), true );
}
}
}
}
/**
* Test scenario:
*
* ComputePathToPose and FollowPath return SUCCESS
* The behavior tree should execute correctly and return SUCCESS
*/
TEST_F( BehaviorTreeTestFixture, TestAllSuccess )
{
// Load behavior tree from file
fs::path bt_file = ament_index_cpp::get_package_share_directory( "nav2_bt_navigator" );
bt_file /= "behavior_trees/";
bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
EXPECT_EQ( bt_handler->loadBehaviorTree( bt_file.string( ) ), true );
BT::NodeStatus result = BT::NodeStatus::RUNNING;
while ( result == BT::NodeStatus::RUNNING ) {
result = bt_handler->tree.tickRoot( );
std::this_thread::sleep_for( 10ms );
}
// The final result should be success since all action servers returned success
EXPECT_EQ( result, BT::NodeStatus::SUCCESS );
// Goal count should be 1 since only one goal is sent to ComputePathToPose and FollowPath servers
EXPECT_EQ( server_handler->compute_path_to_pose_server->getGoalCount( ), 1 );
EXPECT_EQ( server_handler->follow_path_server->getGoalCount( ), 1 );
// Goal count should be 0 since no goal is sent to all other servers
EXPECT_EQ( server_handler->spin_server->getGoalCount( ), 0 );
EXPECT_EQ( server_handler->wait_server->getGoalCount( ), 0 );
EXPECT_EQ( server_handler->backup_server->getGoalCount( ), 0 );
EXPECT_EQ( server_handler->clear_local_costmap_server->getRequestCount( ), 0 );
EXPECT_EQ( server_handler->clear_global_costmap_server->getRequestCount( ), 0 );
}
/**
* Test scenario:
*
* ComputePathToPose returns FAILURE and ClearGlobalCostmap-Context returns FAILURE
* PipelineSequence returns FAILURE and NavigateRecovery triggers RecoveryFallback
* GoalUpdated returns FAILURE and RoundRobin is triggered
* RoundRobin triggers ClearingActions Sequence which returns FAILURE
* RoundRobin triggers Spin, Wait, and BackUp which return FAILURE
* RoundRobin returns FAILURE hence RecoveryCallbackk returns FAILURE
* Finally NavigateRecovery returns FAILURE
* The behavior tree should also return FAILURE
*/
TEST_F( BehaviorTreeTestFixture, TestAllFailure )
{
// Load behavior tree from file
fs::path bt_file = ament_index_cpp::get_package_share_directory( "nav2_bt_navigator" );
bt_file /= "behavior_trees/";
bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
EXPECT_EQ( bt_handler->loadBehaviorTree( bt_file.string( ) ), true );
// Set all action server to fail the first 100 times
std::vector<std::pair<int, int>> failureRange;
failureRange.emplace_back( std::pair<int, int>( 0, 100 ) );
server_handler->compute_path_to_pose_server->setFailureRanges( failureRange );
server_handler->follow_path_server->setFailureRanges( failureRange );
server_handler->spin_server->setFailureRanges( failureRange );
server_handler->wait_server->setFailureRanges( failureRange );
server_handler->backup_server->setFailureRanges( failureRange );
// Disable services
server_handler->clear_global_costmap_server->disable( );
server_handler->clear_local_costmap_server->disable( );
BT::NodeStatus result = BT::NodeStatus::RUNNING;
while ( result == BT::NodeStatus::RUNNING ) {
result = bt_handler->tree.tickRoot( );
std::this_thread::sleep_for( 10ms );
}
// The final result should be failure
EXPECT_EQ( result, BT::NodeStatus::FAILURE );
// Goal count should be 1 since only one goal is sent to ComputePathToPose
EXPECT_EQ( server_handler->compute_path_to_pose_server->getGoalCount( ), 1 );
// Goal count should be 0 since no goal is sent to FollowPath action server
EXPECT_EQ( server_handler->follow_path_server->getGoalCount( ), 0 );
// All recovery action servers were sent 1 goal
EXPECT_EQ( server_handler->spin_server->getGoalCount( ), 1 );
EXPECT_EQ( server_handler->wait_server->getGoalCount( ), 1 );
EXPECT_EQ( server_handler->backup_server->getGoalCount( ), 1 );
// Service count is 0 since the server was disabled
EXPECT_EQ( server_handler->clear_local_costmap_server->getRequestCount( ), 0 );
EXPECT_EQ( server_handler->clear_global_costmap_server->getRequestCount( ), 0 );
}
/**
* Test scenario:
*
* ComputePathToPose returns FAILURE on the first try triggering the planner recovery
* ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns SUCCESS when retried
* FollowPath returns FAILURE on the first try triggering the controller recovery
* ClearLocalCostmap-Context returns SUCCESS and FollowPath returns SUCCESS when retried
* The behavior tree should return SUCCESS
*/
TEST_F( BehaviorTreeTestFixture, TestNavigateSubtreeRecoveries )
{
// Load behavior tree from file
fs::path bt_file = ament_index_cpp::get_package_share_directory( "nav2_bt_navigator" );
bt_file /= "behavior_trees/";
bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
EXPECT_EQ( bt_handler->loadBehaviorTree( bt_file.string( ) ), true );
// Set ComputePathToPose and FollowPath action servers to fail for the first action
std::vector<std::pair<int, int>> failureRange;
failureRange.emplace_back( std::pair<int, int>( 0, 1 ) );
server_handler->compute_path_to_pose_server->setFailureRanges( failureRange );
server_handler->follow_path_server->setFailureRanges( failureRange );
BT::NodeStatus result = BT::NodeStatus::RUNNING;
while ( result == BT::NodeStatus::RUNNING ) {
result = bt_handler->tree.tickRoot( );
std::this_thread::sleep_for( 10ms );
}
// The final result should be success
EXPECT_EQ( result, BT::NodeStatus::SUCCESS );
// Goal count should be 2 since only two goals were sent to ComputePathToPose and FollowPath
EXPECT_EQ( server_handler->compute_path_to_pose_server->getGoalCount( ), 2 );
EXPECT_EQ( server_handler->follow_path_server->getGoalCount( ), 2 );
// Navigate subtree recovery services are called once each
EXPECT_EQ( server_handler->clear_local_costmap_server->getRequestCount( ), 1 );
EXPECT_EQ( server_handler->clear_global_costmap_server->getRequestCount( ), 1 );
// Goal count should be 0 since no goal is sent to all other servers
EXPECT_EQ( server_handler->spin_server->getGoalCount( ), 0 );
EXPECT_EQ( server_handler->wait_server->getGoalCount( ), 0 );
EXPECT_EQ( server_handler->backup_server->getGoalCount( ), 0 );
}
/**
* Test scenario:
*
* ComputePathToPose returns FAILURE on the first try triggering the planner recovery
* ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns SUCCESS when retried
* FollowPath returns FAILURE on the first try triggering the controller recovery
* ClearLocalCostmap-Context returns SUCCESS and FollowPath is retried
* FollowPath returns FAILURE again and PipelineSequence returns FAILURE
* NavigateRecovery triggers RecoveryFallback and GoalUpdated returns FAILURE
* RoundRobin triggers ClearingActions Sequence which returns SUCCESS
* RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
* PipelineSequence is triggered again and ComputePathToPose returns SUCCESS
* FollowPath returns FAILURE on the third try triggering the controller recovery
* ClearLocalCostmap-Context returns SUCCESS and FollowPath returns SUCCESS on the fourth try
* The behavior tree should return SUCCESS
*/
TEST_F( BehaviorTreeTestFixture, TestNavigateRecoverySimple )
{
// Load behavior tree from file
fs::path bt_file = ament_index_cpp::get_package_share_directory( "nav2_bt_navigator" );
bt_file /= "behavior_trees/";
bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
EXPECT_EQ( bt_handler->loadBehaviorTree( bt_file.string( ) ), true );
// Set ComputePathToPose action server to fail for the first action
std::vector<std::pair<int, int>> plannerFailureRange;
plannerFailureRange.emplace_back( std::pair<int, int>( 0, 1 ) );
server_handler->compute_path_to_pose_server->setFailureRanges( plannerFailureRange );
// Set FollowPath action server to fail for the first 3 actions
std::vector<std::pair<int, int>> controllerFailureRange;
controllerFailureRange.emplace_back( std::pair<int, int>( 0, 3 ) );
server_handler->follow_path_server->setFailureRanges( controllerFailureRange );
BT::NodeStatus result = BT::NodeStatus::RUNNING;
while ( result == BT::NodeStatus::RUNNING ) {
result = bt_handler->tree.tickRoot( );
std::this_thread::sleep_for( 10ms );
}
// The final result should be success
EXPECT_EQ( result, BT::NodeStatus::SUCCESS );
// FollowPath is called 4 times
EXPECT_EQ( server_handler->follow_path_server->getGoalCount( ), 4 );
// ComputePathToPose is called 3 times
EXPECT_EQ( server_handler->compute_path_to_pose_server->getGoalCount( ), 3 );
// Local costmap is cleared 3 times
EXPECT_EQ( server_handler->clear_local_costmap_server->getRequestCount( ), 3 );
// Global costmap is cleared 2 times
EXPECT_EQ( server_handler->clear_global_costmap_server->getRequestCount( ), 2 );
// Goal count should be 0 since only no goal is sent to all other servers
EXPECT_EQ( server_handler->spin_server->getGoalCount( ), 0 );
EXPECT_EQ( server_handler->wait_server->getGoalCount( ), 0 );
EXPECT_EQ( server_handler->backup_server->getGoalCount( ), 0 );
}
/**
* Test scenario:
*
* ComputePathToPose returns FAILURE on the first try triggering the planner recovery
* ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried
* PipelineSequence returns FAILURE and NavigateRecovery triggers RecoveryFallback
* GoalUpdated returns FAILURE, RoundRobin triggers ClearingActions Sequence which returns SUCCESS
* RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
*
* PipelineSequence is triggered again and ComputePathToPose returns SUCCESS ( retry #1 )
* FollowPath returns FAILURE on the first try triggering the controller recovery
* ClearLocalCostmap-Context returns SUCCESS and FollowPath is retried
* FollowPath returns FAILURE again and PipelineSequence returns FAILURE
* NavigateRecovery triggers RecoveryFallback and GoalUpdated returns FAILURE
* RoundRobin triggers Spin which returns FAILURE
* RoundRobin triggers Wait which returns SUCCESS
* RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
*
* PipelineSequence is triggered again and ComputePathToPose returns FAILURE ( retry #2 )
* ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried
* PipelineSequence returns FAILURE NavigateRecovery triggers RecoveryFallback
* GoalUpdated returns FAILURE and RoundRobin triggers BackUp which returns FAILURE
* RoundRobin triggers ClearingActions Sequence which returns SUCCESS
* RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
*
* PipelineSequence is triggered again and ComputePathToPose returns FAILURE ( retry #3 )
* ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried
* PipelineSequence returns FAILURE NavigateRecovery triggers RecoveryFallback
* GoalUpdated returns FAILURE and RoundRobin triggers Spin which returns SUCCESS
* RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
*
* PipelineSequence is triggered again and ComputePathToPose returns FAILURE ( retry #4 )
* ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried
* PipelineSequence returns FAILURE NavigateRecovery triggers RecoveryFallback
* GoalUpdated returns FAILURE and RoundRobin triggers Wait which returns FAILURE
* RoundRobin triggers BackUp which returns SUCCESS
* RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
*
* PipelineSequence is triggered again and ComputePathToPose returns SUCCESS ( retry #5 )
* FollowPath returns FAILURE on the first try triggering the controller recovery
* ClearLocalCostmap-Context returns SUCCESS and FollowPath is retried
* FollowPath returns FAILURE again and PipelineSequence returns FAILURE
* NavigateRecovery triggers RecoveryFallback and GoalUpdated returns FAILURE
* RoundRobin triggers ClearingActions Sequence which returns SUCCESS
* RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
*
* PipelineSequence is triggered again and ComputePathToPose returns FAILURE ( retry #6 )
* ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried
* PipelineSequence returns FAILURE and NavigateRecovery finally also returns FAILURE
*
* The behavior tree should return FAILURE
*/
TEST_F( BehaviorTreeTestFixture, TestNavigateRecoveryComplex )
{
// Load behavior tree from file
fs::path bt_file = ament_index_cpp::get_package_share_directory( "nav2_bt_navigator" );
bt_file /= "behavior_trees/";
bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
EXPECT_EQ( bt_handler->loadBehaviorTree( bt_file.string( ) ), true );
// Set ComputePathToPose action server to fail for the first 2 actions
std::vector<std::pair<int, int>> plannerFailureRange;
plannerFailureRange.emplace_back( std::pair<int, int>( 0, 2 ) );
plannerFailureRange.emplace_back( std::pair<int, int>( 4, 9 ) );
plannerFailureRange.emplace_back( std::pair<int, int>( 11, 12 ) );
server_handler->compute_path_to_pose_server->setFailureRanges( plannerFailureRange );
// Set FollowPath action server to fail for the first 2 actions
std::vector<std::pair<int, int>> controllerFailureRange;
controllerFailureRange.emplace_back( std::pair<int, int>( 0, 4 ) );
server_handler->follow_path_server->setFailureRanges( controllerFailureRange );
// Set Spin action server to fail for the first action
std::vector<std::pair<int, int>> spinFailureRange;
spinFailureRange.emplace_back( std::pair<int, int>( 0, 1 ) );
server_handler->spin_server->setFailureRanges( spinFailureRange );
// Set Wait action server to fail for the first action
std::vector<std::pair<int, int>> waitFailureRange;
waitFailureRange.emplace_back( std::pair<int, int>( 2, 2 ) );
server_handler->wait_server->setFailureRanges( waitFailureRange );
// Set BackUp action server to fail for the first action
std::vector<std::pair<int, int>> backupFailureRange;
backupFailureRange.emplace_back( std::pair<int, int>( 0, 1 ) );
server_handler->backup_server->setFailureRanges( backupFailureRange );
BT::NodeStatus result = BT::NodeStatus::RUNNING;
while ( result == BT::NodeStatus::RUNNING ) {
result = bt_handler->tree.tickRoot( );
std::this_thread::sleep_for( 10ms );
}
// The final result should be success
EXPECT_EQ( result, BT::NodeStatus::FAILURE );
// ComputePathToPose is called 12 times
EXPECT_EQ( server_handler->compute_path_to_pose_server->getGoalCount( ), 12 );
// FollowPath is called 4 times
EXPECT_EQ( server_handler->follow_path_server->getGoalCount( ), 4 );
// Local costmap is cleared 5 times
EXPECT_EQ( server_handler->clear_local_costmap_server->getRequestCount( ), 5 );
// Global costmap is cleared 8 times
EXPECT_EQ( server_handler->clear_global_costmap_server->getRequestCount( ), 8 );
// All recovery action servers receive 2 goals
EXPECT_EQ( server_handler->spin_server->getGoalCount( ), 2 );
EXPECT_EQ( server_handler->wait_server->getGoalCount( ), 2 );
EXPECT_EQ( server_handler->backup_server->getGoalCount( ), 2 );
}
/**
* Test scenario:
*
* ComputePathToPose returns FAILURE on the first try triggering the planner recovery
* ClearGlobalCostmap-Context returns SUCCESS and ComputePathToPose returns FAILURE when retried
* PipelineSequence returns FAILURE and NavigateRecovery triggers RecoveryFallback
* GoalUpdated returns FAILURE, RoundRobin triggers ClearingActions Sequence which returns SUCCESS
* RoundRobin returns SUCCESS and RecoveryFallback returns SUCCESS
* PipelineSequence is triggered again and ComputePathToPose returns SUCCESS
* FollowPath returns FAILURE on the first try triggering the controller recovery
* ClearLocalCostmap-Context returns SUCCESS and FollowPath is retried
* FollowPath returns FAILURE and PipelineSequence returns FAILURE
* NavigateRecovery triggers RecoveryFallback which triggers GoalUpdated
* GoalUpdated returns FAILURE and RecoveryFallback triggers RoundRobin
* RoundRobin triggers Spin which returns RUNNING
*
* At this point a new goal is updated on the blackboard
*
* RecoveryFallback triggers GoalUpdated which returns SUCCESS this time
* Since GoalUpdated returned SUCCESS, RoundRobin and hence Spin is halted
* RecoveryFallback also returns SUCCESS and PipelineSequence is retried
* PipelineSequence triggers ComputePathToPose which returns SUCCESS
* FollowPath returns SUCCESS and NavigateRecovery finally also returns SUCCESS
*
* The behavior tree should return SUCCESS
*/
TEST_F( BehaviorTreeTestFixture, TestRecoverySubtreeGoalUpdated )
{
// Load behavior tree from file
fs::path bt_file = ament_index_cpp::get_package_share_directory( "nav2_bt_navigator" );
bt_file /= "behavior_trees/";
bt_file /= "navigate_to_pose_w_replanning_and_recovery.xml";
EXPECT_EQ( bt_handler->loadBehaviorTree( bt_file.string( ) ), true );
// Set ComputePathToPose action server to fail for the first 2 actions
std::vector<std::pair<int, int>> plannerFailureRange;
plannerFailureRange.emplace_back( std::pair<int, int>( 0, 2 ) );
server_handler->compute_path_to_pose_server->setFailureRanges( plannerFailureRange );
// Set FollowPath action server to fail for the first 2 actions
std::vector<std::pair<int, int>> controllerFailureRange;
controllerFailureRange.emplace_back( std::pair<int, int>( 0, 2 ) );
server_handler->follow_path_server->setFailureRanges( controllerFailureRange );
// Set Spin action server to return running for the first action
std::vector<std::pair<int, int>> spinRunningRange;
spinRunningRange.emplace_back( std::pair<int, int>( 1, 1 ) );
server_handler->spin_server->setRunningRanges( spinRunningRange );
BT::NodeStatus result = BT::NodeStatus::RUNNING;
while ( result == BT::NodeStatus::RUNNING ) {
result = bt_handler->tree.tickRoot( );
// Update goal on blackboard after Spin has been triggered once
// to simulate a goal update during a recovery action
if ( server_handler->spin_server->getGoalCount( ) > 0 ) {
geometry_msgs::msg::PoseStamped goal;
goal.pose.position.x = 1.0;
goal.pose.position.y = 1.0;
goal.pose.position.z = 1.0;
goal.pose.orientation.x = 0.0;
goal.pose.orientation.y = 0.0;
goal.pose.orientation.z = 0.0;
goal.pose.orientation.w = 1.0;
bt_handler->blackboard->set<geometry_msgs::msg::PoseStamped>( "goal", goal ); // NOLINT
}
std::this_thread::sleep_for( 10ms );
}
// The final result should be success
EXPECT_EQ( result, BT::NodeStatus::SUCCESS );
// ComputePathToPose is called 4 times
EXPECT_EQ( server_handler->compute_path_to_pose_server->getGoalCount( ), 4 );
// FollowPath is called 3 times
EXPECT_EQ( server_handler->follow_path_server->getGoalCount( ), 3 );
// Local costmap is cleared 2 times
EXPECT_EQ( server_handler->clear_local_costmap_server->getRequestCount( ), 2 );
// Global costmap is cleared 2 times
EXPECT_EQ( server_handler->clear_global_costmap_server->getRequestCount( ), 2 );
// Spin server receives 1 action
EXPECT_EQ( server_handler->spin_server->getGoalCount( ), 1 );
// All recovery action servers receive 0 goals
EXPECT_EQ( server_handler->wait_server->getGoalCount( ), 0 );
EXPECT_EQ( server_handler->backup_server->getGoalCount( ), 0 );
}
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
bool all_successful = RUN_ALL_TESTS( );
return all_successful;
}
1 // Copyright ( c ) 2020 Sarthak Mittal
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <string>
#include <random>
#include <tuple>
#include <memory>
#include <iostream>
#include <chrono>
#include <sstream>
#include <iomanip>
#include "backup_behavior_tester.hpp"
#include "nav2_util/geometry_utils.hpp"
using namespace std::chrono_literals;
using namespace std::chrono; // NOLINT
namespace nav2_system_tests
{
34 BackupBehaviorTester::BackupBehaviorTester( )
: is_active_( false ),
initial_pose_received_( false )
{
node_ = rclcpp::Node::make_shared( "backup_behavior_test" );
tf_buffer_ = std::make_shared<tf2_ros::Buffer>( node_->get_clock( ) );
tf_listener_ = std::make_shared<tf2_ros::TransformListener>( *tf_buffer_ );
client_ptr_ = rclcpp_action::create_client<BackUp>(
node_->get_node_base_interface( ),
node_->get_node_graph_interface( ),
node_->get_node_logging_interface( ),
node_->get_node_waitables_interface( ),
"backup" );
publisher_ =
node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>( "initialpose", 10 );
subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
"amcl_pose", rclcpp::QoS( rclcpp::KeepLast( 1 ) ).transient_local( ).reliable( ),
std::bind( &BackupBehaviorTester::amclPoseCallback, this, std::placeholders::_1 ) );
stamp_ = node_->now( );
}
60 BackupBehaviorTester::~BackupBehaviorTester( )
{
if ( is_active_ ) {
deactivate( );
}
}
67 void BackupBehaviorTester::activate( )
{
if ( is_active_ ) {
throw std::runtime_error( "Trying to activate while already active" );
return;
}
while ( !initial_pose_received_ ) {
RCLCPP_WARN( node_->get_logger( ), "Initial pose not received" );
sendInitialPose( );
std::this_thread::sleep_for( 100ms );
rclcpp::spin_some( node_ );
}
// Wait for lifecycle_manager_navigation to activate behavior_server
std::this_thread::sleep_for( 10s );
if ( !client_ptr_ ) {
RCLCPP_ERROR( node_->get_logger( ), "Action client not initialized" );
is_active_ = false;
return;
}
if ( !client_ptr_->wait_for_action_server( 10s ) ) {
RCLCPP_ERROR( node_->get_logger( ), "Action server not available after waiting" );
is_active_ = false;
return;
}
RCLCPP_INFO( this->node_->get_logger( ), "Backup action server is ready" );
is_active_ = true;
}
100 void BackupBehaviorTester::deactivate( )
{
if ( !is_active_ ) {
throw std::runtime_error( "Trying to deactivate while already inactive" );
}
is_active_ = false;
}
108 bool BackupBehaviorTester::defaultBackupBehaviorTest(
109 const BackUp::Goal goal_msg,
const double tolerance )
{
if ( !is_active_ ) {
RCLCPP_ERROR( node_->get_logger( ), "Not activated" );
return false;
}
// Sleep to let behavior server be ready for serving in multiple runs
std::this_thread::sleep_for( 5s );
RCLCPP_INFO( this->node_->get_logger( ), "Sending goal" );
geometry_msgs::msg::PoseStamped initial_pose;
if ( !nav2_util::getCurrentPose( initial_pose, *tf_buffer_, "odom" ) ) {
RCLCPP_ERROR( node_->get_logger( ), "Current robot pose is not available." );
return false;
}
RCLCPP_INFO( node_->get_logger( ), "Found current robot pose" );
auto goal_handle_future = client_ptr_->async_send_goal( goal_msg );
if ( rclcpp::spin_until_future_complete( node_, goal_handle_future ) !=
rclcpp::FutureReturnCode::SUCCESS )
{
RCLCPP_ERROR( node_->get_logger( ), "send goal call failed :( " );
return false;
}
rclcpp_action::ClientGoalHandle<BackUp>::SharedPtr goal_handle = goal_handle_future.get( );
if ( !goal_handle ) {
RCLCPP_ERROR( node_->get_logger( ), "Goal was rejected by server" );
return false;
}
// Wait for the server to be done with the goal
auto result_future = client_ptr_->async_get_result( goal_handle );
RCLCPP_INFO( node_->get_logger( ), "Waiting for result" );
if ( rclcpp::spin_until_future_complete( node_, result_future ) !=
rclcpp::FutureReturnCode::SUCCESS )
{
RCLCPP_ERROR( node_->get_logger( ), "get result call failed :( " );
return false;
}
rclcpp_action::ClientGoalHandle<BackUp>::WrappedResult wrapped_result = result_future.get( );
switch ( wrapped_result.code ) {
case rclcpp_action::ResultCode::SUCCEEDED: break;
case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(
node_->get_logger( ),
"Goal was aborted" );
return false;
case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(
node_->get_logger( ),
"Goal was canceled" );
return false;
default: RCLCPP_ERROR( node_->get_logger( ), "Unknown result code" );
return false;
}
RCLCPP_INFO( node_->get_logger( ), "result received" );
geometry_msgs::msg::PoseStamped current_pose;
if ( !nav2_util::getCurrentPose( current_pose, *tf_buffer_, "odom" ) ) {
RCLCPP_ERROR( node_->get_logger( ), "Current robot pose is not available." );
return false;
}
double dist = nav2_util::geometry_utils::euclidean_distance( initial_pose, current_pose );
if ( fabs( dist ) > fabs( goal_msg.target.x ) + tolerance ) {
RCLCPP_ERROR(
node_->get_logger( ),
"Distance from goal is %lf ( tolerance %lf )",
fabs( dist - goal_msg.target.x ), tolerance );
return false;
}
return true;
}
192 void BackupBehaviorTester::sendInitialPose( )
{
geometry_msgs::msg::PoseWithCovarianceStamped pose;
pose.header.frame_id = "map";
pose.header.stamp = stamp_;
pose.pose.pose.position.x = -2.0;
pose.pose.pose.position.y = -0.5;
pose.pose.pose.position.z = 0.0;
pose.pose.pose.orientation.x = 0.0;
pose.pose.pose.orientation.y = 0.0;
pose.pose.pose.orientation.z = 0.0;
pose.pose.pose.orientation.w = 1.0;
for ( int i = 0; i < 35; i++ ) {
pose.pose.covariance[i] = 0.0;
}
pose.pose.covariance[0] = 0.08;
pose.pose.covariance[7] = 0.08;
pose.pose.covariance[35] = 0.05;
publisher_->publish( pose );
RCLCPP_INFO( node_->get_logger( ), "Sent initial pose" );
}
215 void BackupBehaviorTester::amclPoseCallback(
216 const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr )
{
initial_pose_received_ = true;
}
} // namespace nav2_system_tests
// Copyright ( c ) 2020 Samsung Research
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef BEHAVIORS__BACKUP__BACKUP_BEHAVIOR_TESTER_HPP_
#define BEHAVIORS__BACKUP__BACKUP_BEHAVIOR_TESTER_HPP_
#include <gtest/gtest.h>
#include <memory>
#include <string>
#include <thread>
#include <algorithm>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "angles/angles.h"
#include "nav2_msgs/action/back_up.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav2_util/node_thread.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
#include "tf2/utils.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
namespace nav2_system_tests
{
41 class BackupBehaviorTester
{
public:
using BackUp = nav2_msgs::action::BackUp;
using GoalHandleBackup = rclcpp_action::ClientGoalHandle<BackUp>;
BackupBehaviorTester( );
~BackupBehaviorTester( );
// Runs a single test with given target yaw
bool defaultBackupBehaviorTest(
const BackUp::Goal goal_msg,
const double tolerance );
void activate( );
void deactivate( );
bool isActive( ) const
{
return is_active_;
}
private:
void sendInitialPose( );
67 void amclPoseCallback( geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr );
bool is_active_;
bool initial_pose_received_;
rclcpp::Time stamp_;
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
rclcpp::Node::SharedPtr node_;
// Publisher to publish initial pose
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr publisher_;
// Subscriber for amcl pose
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
// Action client to call Backup action
rclcpp_action::Client<BackUp>::SharedPtr client_ptr_;
};
} // namespace nav2_system_tests
#endif // BEHAVIORS__BACKUP__BACKUP_BEHAVIOR_TESTER_HPP_
1 // Copyright ( c ) 2020 Samsung Research
// Copyright ( c ) 2020 Sarthak Mittal
// Copyright ( c ) 2022 Joshua Wallace
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <gtest/gtest.h>
#include <cmath>
#include <tuple>
#include <string>
#include <algorithm>
#include "rclcpp/rclcpp.hpp"
#include "backup_behavior_tester.hpp"
#include "nav2_msgs/action/back_up.hpp"
using namespace std::chrono_literals;
using nav2_system_tests::BackupBehaviorTester;
struct TestParameters
{
float x;
float y;
float speed;
float tolerance;
};
41 std::string testNameGenerator( const testing::TestParamInfo<TestParameters> & )
{
static int test_index = 0;
std::string name = "BackUpTest" + std::to_string( test_index );
++test_index;
return name;
}
49 class BackupBehaviorTestFixture
50 : public ::testing::TestWithParam<TestParameters>
{
public:
53 static void SetUpTestCase( )
{
backup_behavior_tester = new BackupBehaviorTester( );
if ( !backup_behavior_tester->isActive( ) ) {
backup_behavior_tester->activate( );
}
}
61 static void TearDownTestCase( )
{
delete backup_behavior_tester;
backup_behavior_tester = nullptr;
}
protected:
68 static BackupBehaviorTester * backup_behavior_tester;
};
BackupBehaviorTester * BackupBehaviorTestFixture::backup_behavior_tester = nullptr;
73 TEST_P( BackupBehaviorTestFixture, testBackupBehavior )
{
auto test_params = GetParam( );
auto goal = nav2_msgs::action::BackUp::Goal( );
goal.target.x = test_params.x;
goal.target.y = test_params.y;
goal.speed = test_params.speed;
float tolerance = test_params.tolerance;
if ( !backup_behavior_tester->isActive( ) ) {
backup_behavior_tester->activate( );
}
bool success = false;
success = backup_behavior_tester->defaultBackupBehaviorTest( goal, tolerance );
float dist_to_obstacle = 2.0f;
if ( ( ( dist_to_obstacle - std::fabs( test_params.x ) ) < std::fabs( goal.speed ) ) ||
std::fabs( goal.target.y ) > 0 )
{
EXPECT_FALSE( success );
} else {
EXPECT_TRUE( success );
}
}
std::vector<TestParameters> test_params = {TestParameters{-0.05, 0.0, -0.2, 0.01},
TestParameters{-0.05, 0.1, -0.2, 0.01},
TestParameters{-2.0, 0.0, -0.2, 0.1}};
104 INSTANTIATE_TEST_SUITE_P(
BackupBehaviorTests,
BackupBehaviorTestFixture,
::testing::Values(
test_params[0],
test_params[1],
test_params[2] ),
testNameGenerator
);
115 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2020 Sarthak Mittal
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <string>
#include <random>
#include <tuple>
#include <memory>
#include <iostream>
#include <chrono>
#include <sstream>
#include <iomanip>
#include "drive_on_heading_behavior_tester.hpp"
#include "nav2_util/geometry_utils.hpp"
using namespace std::chrono_literals;
using namespace std::chrono; // NOLINT
namespace nav2_system_tests
{
34 DriveOnHeadingBehaviorTester::DriveOnHeadingBehaviorTester( )
: is_active_( false ),
initial_pose_received_( false )
{
node_ = rclcpp::Node::make_shared( "DriveOnHeading_behavior_test" );
tf_buffer_ = std::make_shared<tf2_ros::Buffer>( node_->get_clock( ) );
tf_listener_ = std::make_shared<tf2_ros::TransformListener>( *tf_buffer_ );
client_ptr_ = rclcpp_action::create_client<DriveOnHeading>(
node_->get_node_base_interface( ),
node_->get_node_graph_interface( ),
node_->get_node_logging_interface( ),
node_->get_node_waitables_interface( ),
"drive_on_heading" );
publisher_ =
node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>( "initialpose", 10 );
subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
"amcl_pose", rclcpp::QoS( rclcpp::KeepLast( 1 ) ).transient_local( ).reliable( ),
std::bind( &DriveOnHeadingBehaviorTester::amclPoseCallback, this, std::placeholders::_1 ) );
stamp_ = node_->now( );
}
60 DriveOnHeadingBehaviorTester::~DriveOnHeadingBehaviorTester( )
{
if ( is_active_ ) {
deactivate( );
}
}
67 void DriveOnHeadingBehaviorTester::activate( )
{
if ( is_active_ ) {
throw std::runtime_error( "Trying to activate while already active" );
return;
}
while ( !initial_pose_received_ ) {
RCLCPP_WARN( node_->get_logger( ), "Initial pose not received" );
sendInitialPose( );
std::this_thread::sleep_for( 100ms );
rclcpp::spin_some( node_ );
}
// Wait for lifecycle_manager_navigation to activate behavior_server
std::this_thread::sleep_for( 10s );
if ( !client_ptr_ ) {
RCLCPP_ERROR( node_->get_logger( ), "Action client not initialized" );
is_active_ = false;
return;
}
if ( !client_ptr_->wait_for_action_server( 10s ) ) {
RCLCPP_ERROR( node_->get_logger( ), "Action server not available after waiting" );
is_active_ = false;
return;
}
RCLCPP_INFO( this->node_->get_logger( ), "DriveOnHeading action server is ready" );
is_active_ = true;
}
100 void DriveOnHeadingBehaviorTester::deactivate( )
{
if ( !is_active_ ) {
throw std::runtime_error( "Trying to deactivate while already inactive" );
}
is_active_ = false;
}
108 bool DriveOnHeadingBehaviorTester::defaultDriveOnHeadingBehaviorTest(
109 const DriveOnHeading::Goal goal_msg,
const double tolerance )
{
if ( !is_active_ ) {
RCLCPP_ERROR( node_->get_logger( ), "Not activated" );
return false;
}
// Sleep to let behavior server be ready for serving in multiple runs
std::this_thread::sleep_for( 5s );
RCLCPP_INFO( this->node_->get_logger( ), "Sending goal" );
geometry_msgs::msg::PoseStamped initial_pose;
if ( !nav2_util::getCurrentPose( initial_pose, *tf_buffer_, "odom" ) ) {
RCLCPP_ERROR( node_->get_logger( ), "Current robot pose is not available." );
return false;
}
RCLCPP_INFO( node_->get_logger( ), "Found current robot pose" );
auto goal_handle_future = client_ptr_->async_send_goal( goal_msg );
if ( rclcpp::spin_until_future_complete( node_, goal_handle_future ) !=
rclcpp::FutureReturnCode::SUCCESS )
{
RCLCPP_ERROR( node_->get_logger( ), "send goal call failed :( " );
return false;
}
rclcpp_action::ClientGoalHandle<DriveOnHeading>::SharedPtr goal_handle = goal_handle_future.get( );
if ( !goal_handle ) {
RCLCPP_ERROR( node_->get_logger( ), "Goal was rejected by server" );
return false;
}
// Wait for the server to be done with the goal
auto result_future = client_ptr_->async_get_result( goal_handle );
RCLCPP_INFO( node_->get_logger( ), "Waiting for result" );
if ( rclcpp::spin_until_future_complete( node_, result_future ) !=
rclcpp::FutureReturnCode::SUCCESS )
{
RCLCPP_ERROR( node_->get_logger( ), "get result call failed :( " );
return false;
}
rclcpp_action::ClientGoalHandle<DriveOnHeading>::WrappedResult wrapped_result =
result_future.get( );
switch ( wrapped_result.code ) {
case rclcpp_action::ResultCode::SUCCEEDED: break;
case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(
node_->get_logger( ),
"Goal was aborted" );
return false;
case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(
node_->get_logger( ),
"Goal was canceled" );
return false;
default: RCLCPP_ERROR( node_->get_logger( ), "Unknown result code" );
return false;
}
RCLCPP_INFO( node_->get_logger( ), "result received" );
geometry_msgs::msg::PoseStamped current_pose;
if ( !nav2_util::getCurrentPose( current_pose, *tf_buffer_, "odom" ) ) {
RCLCPP_ERROR( node_->get_logger( ), "Current robot pose is not available." );
return false;
}
double dist = nav2_util::geometry_utils::euclidean_distance( initial_pose, current_pose );
if ( fabs( dist ) > fabs( goal_msg.target.x ) + tolerance ) {
RCLCPP_ERROR(
node_->get_logger( ),
"Distance from goal is %lf ( tolerance %lf )",
fabs( dist - goal_msg.target.x ), tolerance );
return false;
}
return true;
}
193 void DriveOnHeadingBehaviorTester::sendInitialPose( )
{
geometry_msgs::msg::PoseWithCovarianceStamped pose;
pose.header.frame_id = "map";
pose.header.stamp = stamp_;
pose.pose.pose.position.x = -2.0;
pose.pose.pose.position.y = -0.5;
pose.pose.pose.position.z = 0.0;
pose.pose.pose.orientation.x = 0.0;
pose.pose.pose.orientation.y = 0.0;
pose.pose.pose.orientation.z = 0.0;
pose.pose.pose.orientation.w = 1.0;
for ( int i = 0; i < 35; i++ ) {
pose.pose.covariance[i] = 0.0;
}
pose.pose.covariance[0] = 0.08;
pose.pose.covariance[7] = 0.08;
pose.pose.covariance[35] = 0.05;
publisher_->publish( pose );
RCLCPP_INFO( node_->get_logger( ), "Sent initial pose" );
}
216 void DriveOnHeadingBehaviorTester::amclPoseCallback(
217 const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr )
{
initial_pose_received_ = true;
}
} // namespace nav2_system_tests
// Copyright ( c ) 2020 Samsung Research
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef BEHAVIORS__DRIVE_ON_HEADING__DRIVE_ON_HEADING_BEHAVIOR_TESTER_HPP_
#define BEHAVIORS__DRIVE_ON_HEADING__DRIVE_ON_HEADING_BEHAVIOR_TESTER_HPP_
#include <gtest/gtest.h>
#include <memory>
#include <string>
#include <thread>
#include <algorithm>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "angles/angles.h"
#include "nav2_msgs/action/drive_on_heading.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav2_util/node_thread.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
#include "tf2/utils.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
namespace nav2_system_tests
{
41 class DriveOnHeadingBehaviorTester
{
public:
using DriveOnHeading = nav2_msgs::action::DriveOnHeading;
using GoalHandleDriveOnHeading = rclcpp_action::ClientGoalHandle<DriveOnHeading>;
DriveOnHeadingBehaviorTester( );
~DriveOnHeadingBehaviorTester( );
// Runs a single test with given target yaw
bool defaultDriveOnHeadingBehaviorTest(
const DriveOnHeading::Goal goal_msg,
double tolerance );
void activate( );
void deactivate( );
bool isActive( ) const
{
return is_active_;
}
private:
void sendInitialPose( );
67 void amclPoseCallback( geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr );
bool is_active_;
bool initial_pose_received_;
rclcpp::Time stamp_;
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
rclcpp::Node::SharedPtr node_;
// Publisher to publish initial pose
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr publisher_;
// Subscriber for amcl pose
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
// Action client to call DriveOnHeading action
rclcpp_action::Client<DriveOnHeading>::SharedPtr client_ptr_;
};
} // namespace nav2_system_tests
#endif // BEHAVIORS__DRIVE_ON_HEADING__DRIVE_ON_HEADING_BEHAVIOR_TESTER_HPP_
1 // Copyright ( c ) 2020 Samsung Research
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <gtest/gtest.h>
#include <cmath>
#include <tuple>
#include <string>
#include <algorithm>
#include "rclcpp/rclcpp.hpp"
#include "drive_on_heading_behavior_tester.hpp"
using namespace std::chrono_literals;
using nav2_system_tests::DriveOnHeadingBehaviorTester;
struct TestParameters
{
float x;
float y;
float speed;
float time_allowance;
float tolerance;
};
39 std::string testNameGenerator( const testing::TestParamInfo<TestParameters> & )
{
static int test_index = 0;
std::string name = "DriveOnHeadingTest" + std::to_string( test_index );
++test_index;
return name;
}
47 class DriveOnHeadingBehaviorTestFixture
48 : public ::testing::TestWithParam<TestParameters>
{
public:
51 static void SetUpTestCase( )
{
drive_on_heading_behavior_tester = new DriveOnHeadingBehaviorTester( );
if ( !drive_on_heading_behavior_tester->isActive( ) ) {
drive_on_heading_behavior_tester->activate( );
}
}
59 static void TearDownTestCase( )
{
delete drive_on_heading_behavior_tester;
drive_on_heading_behavior_tester = nullptr;
}
protected:
66 static DriveOnHeadingBehaviorTester * drive_on_heading_behavior_tester;
};
DriveOnHeadingBehaviorTester * DriveOnHeadingBehaviorTestFixture::drive_on_heading_behavior_tester =
nullptr;
72 TEST_P( DriveOnHeadingBehaviorTestFixture, testBackupBehavior )
{
auto test_params = GetParam( );
auto goal = nav2_msgs::action::DriveOnHeading::Goal( );
goal.target.x = test_params.x;
goal.target.y = test_params.y;
goal.speed = test_params.speed;
goal.time_allowance.sec = test_params.time_allowance;
float tolerance = test_params.tolerance;
if ( !drive_on_heading_behavior_tester->isActive( ) ) {
drive_on_heading_behavior_tester->activate( );
}
bool success = false;
success = drive_on_heading_behavior_tester->defaultDriveOnHeadingBehaviorTest(
goal,
tolerance );
float dist_to_obstacle = 2.0f;
if ( ( ( dist_to_obstacle - std::fabs( test_params.x ) ) < std::fabs( goal.speed ) ) ||
std::fabs( goal.target.y ) > 0 ||
goal.time_allowance.sec < 2.0 ||
!( ( goal.target.x > 0.0 ) == ( goal.speed > 0.0 ) ) )
{
EXPECT_FALSE( success );
} else {
EXPECT_TRUE( success );
}
}
std::vector<TestParameters> test_params = {TestParameters{-0.05, 0.0, -0.2, 10.0, 0.01},
TestParameters{-0.05, 0.1, -0.2, 10.0, 0.01},
TestParameters{-2.0, 0.0, -0.2, 10.0, 0.1},
TestParameters{-0.05, 0.0, -0.01, 1.0, 0.01},
TestParameters{0.05, 0.0, -0.2, 10.0, 0.01}};
109 INSTANTIATE_TEST_SUITE_P(
DriveOnHeadingBehaviorTests,
DriveOnHeadingBehaviorTestFixture,
::testing::Values(
test_params[0],
test_params[1],
test_params[2],
test_params[3],
test_params[4] ),
testNameGenerator );
120 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2020 Sarthak Mittal
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <string>
#include <random>
#include <tuple>
#include <memory>
#include <iostream>
#include <chrono>
#include <sstream>
#include <iomanip>
#include "spin_behavior_tester.hpp"
using namespace std::chrono_literals;
using namespace std::chrono; // NOLINT
namespace nav2_system_tests
{
33 SpinBehaviorTester::SpinBehaviorTester( )
: is_active_( false ),
initial_pose_received_( false )
{
node_ = rclcpp::Node::make_shared( "spin_behavior_test" );
tf_buffer_ = std::make_shared<tf2_ros::Buffer>( node_->get_clock( ) );
tf_listener_ = std::make_shared<tf2_ros::TransformListener>( *tf_buffer_ );
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>( node_ );
if ( std::getenv( "MAKE_FAKE_COSTMAP" ) != NULL ) {
// if this variable is set, make a fake costmap
make_fake_costmap_ = true;
} else {
make_fake_costmap_ = false;
}
client_ptr_ = rclcpp_action::create_client<Spin>(
node_->get_node_base_interface( ),
node_->get_node_graph_interface( ),
node_->get_node_logging_interface( ),
node_->get_node_waitables_interface( ),
"spin" );
publisher_ =
node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>( "initialpose", 10 );
fake_costmap_publisher_ =
node_->create_publisher<nav2_msgs::msg::Costmap>(
"local_costmap/costmap_raw",
rclcpp::QoS( rclcpp::KeepLast( 1 ) ).transient_local( ).reliable( ) );
fake_footprint_publisher_ = node_->create_publisher<geometry_msgs::msg::PolygonStamped>(
"local_costmap/published_footprint", rclcpp::SystemDefaultsQoS( ) );
subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
"amcl_pose", rclcpp::QoS( rclcpp::KeepLast( 1 ) ).transient_local( ).reliable( ),
std::bind( &SpinBehaviorTester::amclPoseCallback, this, std::placeholders::_1 ) );
stamp_ = node_->now( );
}
73 SpinBehaviorTester::~SpinBehaviorTester( )
{
if ( is_active_ ) {
deactivate( );
}
}
80 void SpinBehaviorTester::activate( )
{
if ( is_active_ ) {
throw std::runtime_error( "Trying to activate while already active" );
return;
}
if ( !make_fake_costmap_ ) {
while ( !initial_pose_received_ ) {
RCLCPP_WARN( node_->get_logger( ), "Initial pose not received" );
sendInitialPose( );
std::this_thread::sleep_for( 100ms );
rclcpp::spin_some( node_ );
}
} else {
sendFakeOdom( 0.0 );
}
// Wait for lifecycle_manager_navigation to activate behavior_server
std::this_thread::sleep_for( 10s );
if ( !client_ptr_ ) {
RCLCPP_ERROR( node_->get_logger( ), "Action client not initialized" );
is_active_ = false;
return;
}
if ( !client_ptr_->wait_for_action_server( 10s ) ) {
RCLCPP_ERROR( node_->get_logger( ), "Action server not available after waiting" );
is_active_ = false;
return;
}
RCLCPP_INFO( this->node_->get_logger( ), "Spin action server is ready" );
is_active_ = true;
}
116 void SpinBehaviorTester::deactivate( )
{
if ( !is_active_ ) {
throw std::runtime_error( "Trying to deactivate while already inactive" );
}
is_active_ = false;
}
124 bool SpinBehaviorTester::defaultSpinBehaviorTest(
const float target_yaw,
const double tolerance )
{
if ( !is_active_ ) {
RCLCPP_ERROR( node_->get_logger( ), "Not activated" );
return false;
}
// Sleep to let behavior server be ready for serving in multiple runs
std::this_thread::sleep_for( 5s );
if ( make_fake_costmap_ ) {
sendFakeOdom( 0.0 );
}
auto goal_msg = Spin::Goal( );
goal_msg.target_yaw = target_yaw;
// Intialize fake costmap
if ( make_fake_costmap_ ) {
sendFakeCostmap( target_yaw );
sendFakeOdom( 0.0 );
}
geometry_msgs::msg::PoseStamped initial_pose;
if ( !nav2_util::getCurrentPose( initial_pose, *tf_buffer_, "odom" ) ) {
RCLCPP_ERROR( node_->get_logger( ), "Current robot pose is not available." );
return false;
}
RCLCPP_INFO( node_->get_logger( ), "Found current robot pose" );
RCLCPP_INFO(
node_->get_logger( ),
"Init Yaw is %lf",
fabs( tf2::getYaw( initial_pose.pose.orientation ) ) );
RCLCPP_INFO( node_->get_logger( ), "Before sending goal" );
// Intialize fake costmap
if ( make_fake_costmap_ ) {
sendFakeCostmap( target_yaw );
sendFakeOdom( 0.0 );
}
rclcpp::sleep_for( std::chrono::milliseconds( 100 ) );
auto goal_handle_future = client_ptr_->async_send_goal( goal_msg );
if ( rclcpp::spin_until_future_complete( node_, goal_handle_future ) !=
rclcpp::FutureReturnCode::SUCCESS )
{
RCLCPP_ERROR( node_->get_logger( ), "send goal call failed :( " );
return false;
}
rclcpp_action::ClientGoalHandle<Spin>::SharedPtr goal_handle = goal_handle_future.get( );
if ( !goal_handle ) {
RCLCPP_ERROR( node_->get_logger( ), "Goal was rejected by server" );
return false;
}
// Wait for the server to be done with the goal
auto result_future = client_ptr_->async_get_result( goal_handle );
RCLCPP_INFO( node_->get_logger( ), "Waiting for result" );
rclcpp::sleep_for( std::chrono::milliseconds( 1000 ) );
if ( make_fake_costmap_ ) { // if we are faking the costmap, we will fake success.
sendFakeOdom( 0.0 );
sendFakeCostmap( target_yaw );
RCLCPP_INFO( node_->get_logger( ), "target_yaw %lf", target_yaw );
// Slowly increment command yaw by increment to simulate the robot slowly spinning into place
float step_size = tolerance / 4.0;
for ( float command_yaw = 0.0;
abs( command_yaw ) < abs( target_yaw );
command_yaw = command_yaw + step_size )
{
sendFakeOdom( command_yaw );
sendFakeCostmap( target_yaw );
rclcpp::sleep_for( std::chrono::milliseconds( 1 ) );
}
sendFakeOdom( target_yaw );
sendFakeCostmap( target_yaw );
RCLCPP_INFO( node_->get_logger( ), "After sending goal" );
}
if ( rclcpp::spin_until_future_complete( node_, result_future ) !=
rclcpp::FutureReturnCode::SUCCESS )
{
RCLCPP_ERROR( node_->get_logger( ), "get result call failed :( " );
return false;
}
rclcpp_action::ClientGoalHandle<Spin>::WrappedResult wrapped_result = result_future.get( );
switch ( wrapped_result.code ) {
case rclcpp_action::ResultCode::SUCCEEDED: break;
case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(
node_->get_logger( ),
"Goal was aborted" );
return false;
case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(
node_->get_logger( ),
"Goal was canceled" );
return false;
default: RCLCPP_ERROR( node_->get_logger( ), "Unknown result code" );
return false;
}
RCLCPP_INFO( node_->get_logger( ), "result received" );
geometry_msgs::msg::PoseStamped current_pose;
if ( !nav2_util::getCurrentPose( current_pose, *tf_buffer_, "odom" ) ) {
RCLCPP_ERROR( node_->get_logger( ), "Current robot pose is not available." );
return false;
}
double goal_yaw = angles::normalize_angle(
tf2::getYaw( initial_pose.pose.orientation ) + target_yaw );
double dyaw = angles::shortest_angular_distance(
goal_yaw, tf2::getYaw( current_pose.pose.orientation ) );
if ( fabs( dyaw ) > tolerance ) {
RCLCPP_ERROR(
node_->get_logger( ),
"Init Yaw is %lf ( tolerance %lf )",
fabs( tf2::getYaw( initial_pose.pose.orientation ) ), tolerance );
RCLCPP_ERROR(
node_->get_logger( ),
"Current Yaw is %lf ( tolerance %lf )",
fabs( tf2::getYaw( current_pose.pose.orientation ) ), tolerance );
RCLCPP_ERROR(
node_->get_logger( ),
"Angular distance from goal is %lf ( tolerance %lf )",
fabs( dyaw ), tolerance );
return false;
}
return true;
}
263 void SpinBehaviorTester::sendFakeCostmap( float angle )
{
nav2_msgs::msg::Costmap fake_costmap;
fake_costmap.header.frame_id = "odom";
fake_costmap.header.stamp = stamp_;
fake_costmap.metadata.layer = "master";
fake_costmap.metadata.resolution = .1;
fake_costmap.metadata.size_x = 100;
fake_costmap.metadata.size_y = 100;
fake_costmap.metadata.origin.position.x = 0;
fake_costmap.metadata.origin.position.y = 0;
fake_costmap.metadata.origin.orientation.w = 1.0;
float costmap_val = 0;
for ( int ix = 0; ix < 100; ix++ ) {
for ( int iy = 0; iy < 100; iy++ ) {
if ( abs( angle ) > M_PI_2f32 ) {
// fake obstacles in the way so we get failure due to potential collision
costmap_val = 100;
}
fake_costmap.data.push_back( costmap_val );
}
}
fake_costmap_publisher_->publish( fake_costmap );
}
289 void SpinBehaviorTester::sendInitialPose( )
{
geometry_msgs::msg::PoseWithCovarianceStamped pose;
pose.header.frame_id = "map";
pose.header.stamp = stamp_;
pose.pose.pose.position.x = -2.0;
pose.pose.pose.position.y = -0.5;
pose.pose.pose.position.z = 0.0;
pose.pose.pose.orientation.x = 0.0;
pose.pose.pose.orientation.y = 0.0;
pose.pose.pose.orientation.z = 0.0;
pose.pose.pose.orientation.w = 1.0;
for ( int i = 0; i < 35; i++ ) {
pose.pose.covariance[i] = 0.0;
}
pose.pose.covariance[0] = 0.08;
pose.pose.covariance[7] = 0.08;
pose.pose.covariance[35] = 0.05;
publisher_->publish( pose );
RCLCPP_INFO( node_->get_logger( ), "Sent initial pose" );
}
312 void SpinBehaviorTester::sendFakeOdom( float angle )
{
geometry_msgs::msg::TransformStamped transformStamped;
transformStamped.header.stamp = stamp_;
transformStamped.header.frame_id = "odom";
transformStamped.child_frame_id = "base_link";
transformStamped.transform.translation.x = 0.0;
transformStamped.transform.translation.y = 0.0;
transformStamped.transform.translation.z = 0.0;
tf2::Quaternion q;
q.setRPY( 0, 0, angle );
transformStamped.transform.rotation.x = q.x( );
transformStamped.transform.rotation.y = q.y( );
transformStamped.transform.rotation.z = q.z( );
transformStamped.transform.rotation.w = q.w( );
tf_broadcaster_->sendTransform( transformStamped );
geometry_msgs::msg::PolygonStamped footprint;
footprint.header.frame_id = "odom";
footprint.header.stamp = stamp_;
footprint.polygon.points.resize( 4 );
footprint.polygon.points[0].x = 0.22;
footprint.polygon.points[0].y = 0.22;
footprint.polygon.points[1].x = 0.22;
footprint.polygon.points[1].y = -0.22;
footprint.polygon.points[2].x = -0.22;
footprint.polygon.points[2].y = -0.22;
footprint.polygon.points[3].x = -0.22;
footprint.polygon.points[3].y = 0.22;
fake_footprint_publisher_->publish( footprint );
}
345 void SpinBehaviorTester::amclPoseCallback(
346 const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr )
{
initial_pose_received_ = true;
}
} // namespace nav2_system_tests
// Copyright ( c ) 2020 Sarthak Mittal
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef BEHAVIORS__SPIN__SPIN_BEHAVIOR_TESTER_HPP_
#define BEHAVIORS__SPIN__SPIN_BEHAVIOR_TESTER_HPP_
#include <gtest/gtest.h>
#include <memory>
#include <string>
#include <thread>
#include <algorithm>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "angles/angles.h"
#include "nav2_msgs/action/spin.hpp"
#include "nav2_msgs/msg/costmap.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav2_util/node_thread.hpp"
#include "geometry_msgs/msg/point32.hpp"
#include "geometry_msgs/msg/polygon_stamped.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "tf2/utils.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/transform_listener.h"
namespace nav2_system_tests
{
48 class SpinBehaviorTester
{
public:
using Spin = nav2_msgs::action::Spin;
using GoalHandleSpin = rclcpp_action::ClientGoalHandle<Spin>;
SpinBehaviorTester( );
~SpinBehaviorTester( );
// Runs a single test with given target yaw
bool defaultSpinBehaviorTest(
float target_yaw,
double tolerance = 0.1 );
void activate( );
void deactivate( );
bool isActive( ) const
{
return is_active_;
}
private:
void sendInitialPose( );
74 void sendFakeCostmap( float angle );
75 void sendFakeOdom( float angle );
77 void amclPoseCallback( geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr );
bool is_active_;
bool initial_pose_received_;
bool make_fake_costmap_;
rclcpp::Time stamp_;
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
rclcpp::Node::SharedPtr node_;
// Publisher to publish initial pose
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr publisher_;
// Publisher to publish fake costmap raw
rclcpp::Publisher<nav2_msgs::msg::Costmap>::SharedPtr fake_costmap_publisher_;
// Publisher to publish fake costmap footprint
rclcpp::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr fake_footprint_publisher_;
// Subscriber for amcl pose
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
// Action client to call spin action
rclcpp_action::Client<Spin>::SharedPtr client_ptr_;
};
} // namespace nav2_system_tests
#endif // BEHAVIORS__SPIN__SPIN_BEHAVIOR_TESTER_HPP_
// Copyright ( c ) 2020 Samsung Research
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <cmath>
#include <tuple>
#include <string>
#include <algorithm>
#include "rclcpp/rclcpp.hpp"
#include "spin_behavior_tester.hpp"
using namespace std::chrono_literals;
using nav2_system_tests::SpinBehaviorTester;
29 std::string testNameGenerator( const testing::TestParamInfo<std::tuple<float, float>> & param )
{
std::string name = std::to_string( std::abs( std::get<0>( param.param ) ) ) + "_" + std::to_string(
std::get<1>( param.param ) );
name.erase( std::remove( name.begin( ), name.end( ), '.' ), name.end( ) );
return name;
}
37 class SpinBehaviorTestFixture
38 : public ::testing::TestWithParam<std::tuple<float, float>>
{
public:
static void SetUpTestCase( )
{
spin_recovery_tester = new SpinBehaviorTester( );
if ( !spin_recovery_tester->isActive( ) ) {
spin_recovery_tester->activate( );
}
}
static void TearDownTestCase( )
{
delete spin_recovery_tester;
spin_recovery_tester = nullptr;
}
protected:
static SpinBehaviorTester * spin_recovery_tester;
};
SpinBehaviorTester * SpinBehaviorTestFixture::spin_recovery_tester = nullptr;
TEST_P( SpinBehaviorTestFixture, testSpinRecovery )
{
float target_yaw = std::get<0>( GetParam( ) );
float tolerance = std::get<1>( GetParam( ) );
bool success = false;
int num_tries = 3;
for ( int i = 0; i != num_tries; i++ ) {
success = success || spin_recovery_tester->defaultSpinBehaviorTest( target_yaw, tolerance );
if ( success ) {
break;
}
}
if ( std::getenv( "MAKE_FAKE_COSTMAP" ) != NULL && abs( target_yaw ) > M_PI_2f32 ) {
// if this variable is set, make a fake costmap
// in the fake spin test, we expect a collision for angles > M_PI_2
77 EXPECT_EQ( false, success );
} else {
EXPECT_EQ( true, success );
}
}
INSTANTIATE_TEST_SUITE_P(
SpinRecoveryTests,
SpinBehaviorTestFixture,
::testing::Values(
std::make_tuple( -M_PIf32 / 6.0, 0.1 ),
std::make_tuple( M_PI_4f32, 0.1 ),
std::make_tuple( -M_PI_2f32, 0.1 ),
std::make_tuple( M_PIf32, 0.1 ),
std::make_tuple( 3.0 * M_PIf32 / 2.0, 0.15 ),
std::make_tuple( -2.0 * M_PIf32, 0.1 ),
std::make_tuple( 4.0 * M_PIf32, 0.15 ) ),
testNameGenerator );
int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2020 Samsung Research
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <gtest/gtest.h>
#include <cmath>
#include <tuple>
#include <string>
#include <algorithm>
#include "rclcpp/rclcpp.hpp"
#include "wait_behavior_tester.hpp"
using namespace std::chrono_literals;
using nav2_system_tests::WaitBehaviorTester;
30 std::string testNameGenerator( const testing::TestParamInfo<std::tuple<float, float>> & param )
{
std::string name = std::to_string( std::abs( std::get<0>( param.param ) ) ) + "_" + std::to_string(
std::get<1>( param.param ) );
name.erase( std::remove( name.begin( ), name.end( ), '.' ), name.end( ) );
return name;
}
38 class WaitBehaviorTestFixture
39 : public ::testing::TestWithParam<std::tuple<float, float>>
{
public:
static void SetUpTestCase( )
{
wait_behavior_tester = new WaitBehaviorTester( );
if ( !wait_behavior_tester->isActive( ) ) {
wait_behavior_tester->activate( );
}
}
static void TearDownTestCase( )
{
delete wait_behavior_tester;
wait_behavior_tester = nullptr;
}
protected:
static WaitBehaviorTester * wait_behavior_tester;
};
WaitBehaviorTester * WaitBehaviorTestFixture::wait_behavior_tester = nullptr;
62 TEST_P( WaitBehaviorTestFixture, testSWaitBehavior )
{
float wait_time = std::get<0>( GetParam( ) );
float cancel = std::get<1>( GetParam( ) );
bool success = false;
int num_tries = 3;
for ( int i = 0; i != num_tries; i++ ) {
if ( cancel == 1.0 ) {
success = success || wait_behavior_tester->behaviorTestCancel( wait_time );
} else {
success = success || wait_behavior_tester->behaviorTest( wait_time );
}
if ( success ) {
break;
}
}
EXPECT_EQ( true, success );
}
83 INSTANTIATE_TEST_SUITE_P(
WaitBehaviorTests,
WaitBehaviorTestFixture,
::testing::Values(
std::make_tuple( 1.0, 0.0 ),
std::make_tuple( 2.0, 0.0 ),
std::make_tuple( 5.0, 0.0 ),
std::make_tuple( 10.0, 1.0 ) ),
testNameGenerator );
93 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2020 Samsung Research
// Copyright ( c ) 2020 Sarthak Mittal
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <string>
#include <random>
#include <tuple>
#include <memory>
#include <iostream>
#include <chrono>
#include <sstream>
#include <iomanip>
#include "wait_behavior_tester.hpp"
using namespace std::chrono_literals;
using namespace std::chrono; // NOLINT
namespace nav2_system_tests
{
34 WaitBehaviorTester::WaitBehaviorTester( )
: is_active_( false ),
initial_pose_received_( false )
{
node_ = rclcpp::Node::make_shared( "wait_behavior_test" );
tf_buffer_ = std::make_shared<tf2_ros::Buffer>( node_->get_clock( ) );
tf_listener_ = std::make_shared<tf2_ros::TransformListener>( *tf_buffer_ );
client_ptr_ = rclcpp_action::create_client<Wait>(
node_->get_node_base_interface( ),
node_->get_node_graph_interface( ),
node_->get_node_logging_interface( ),
node_->get_node_waitables_interface( ),
"wait" );
publisher_ =
node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>( "initialpose", 10 );
subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
"amcl_pose", rclcpp::QoS( rclcpp::KeepLast( 1 ) ).transient_local( ).reliable( ),
std::bind( &WaitBehaviorTester::amclPoseCallback, this, std::placeholders::_1 ) );
}
58 WaitBehaviorTester::~WaitBehaviorTester( )
{
if ( is_active_ ) {
deactivate( );
}
}
65 void WaitBehaviorTester::activate( )
{
if ( is_active_ ) {
throw std::runtime_error( "Trying to activate while already active" );
return;
}
while ( !initial_pose_received_ ) {
RCLCPP_WARN( node_->get_logger( ), "Initial pose not received" );
sendInitialPose( );
std::this_thread::sleep_for( 100ms );
rclcpp::spin_some( node_ );
}
// Wait for lifecycle_manager_navigation to activate behavior_server
std::this_thread::sleep_for( 10s );
if ( !client_ptr_ ) {
RCLCPP_ERROR( node_->get_logger( ), "Action client not initialized" );
is_active_ = false;
return;
}
if ( !client_ptr_->wait_for_action_server( 10s ) ) {
RCLCPP_ERROR( node_->get_logger( ), "Action server not available after waiting" );
is_active_ = false;
return;
}
RCLCPP_INFO( this->node_->get_logger( ), "Wait action server is ready" );
is_active_ = true;
}
98 void WaitBehaviorTester::deactivate( )
{
if ( !is_active_ ) {
throw std::runtime_error( "Trying to deactivate while already inactive" );
}
is_active_ = false;
}
106 bool WaitBehaviorTester::behaviorTest(
const float wait_time )
{
if ( !is_active_ ) {
RCLCPP_ERROR( node_->get_logger( ), "Not activated" );
return false;
}
// Sleep to let behavior server be ready for serving in multiple runs
std::this_thread::sleep_for( 5s );
auto start_time = node_->now( );
auto goal_msg = Wait::Goal( );
goal_msg.time = rclcpp::Duration( wait_time, 0.0 );
RCLCPP_INFO( this->node_->get_logger( ), "Sending goal" );
auto goal_handle_future = client_ptr_->async_send_goal( goal_msg );
if ( rclcpp::spin_until_future_complete( node_, goal_handle_future ) !=
rclcpp::FutureReturnCode::SUCCESS )
{
RCLCPP_ERROR( node_->get_logger( ), "send goal call failed :( " );
return false;
}
rclcpp_action::ClientGoalHandle<Wait>::SharedPtr goal_handle = goal_handle_future.get( );
if ( !goal_handle ) {
RCLCPP_ERROR( node_->get_logger( ), "Goal was rejected by server" );
return false;
}
// Wait for the server to be done with the goal
auto result_future = client_ptr_->async_get_result( goal_handle );
RCLCPP_INFO( node_->get_logger( ), "Waiting for result" );
if ( rclcpp::spin_until_future_complete( node_, result_future ) !=
rclcpp::FutureReturnCode::SUCCESS )
{
RCLCPP_ERROR( node_->get_logger( ), "get result call failed :( " );
return false;
}
rclcpp_action::ClientGoalHandle<Wait>::WrappedResult wrapped_result = result_future.get( );
switch ( wrapped_result.code ) {
case rclcpp_action::ResultCode::SUCCEEDED: break;
case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(
node_->get_logger( ),
"Goal was aborted" );
return false;
case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(
node_->get_logger( ),
"Goal was canceled" );
return false;
default: RCLCPP_ERROR( node_->get_logger( ), "Unknown result code" );
return false;
}
RCLCPP_INFO( node_->get_logger( ), "result received" );
if ( ( node_->now( ) - start_time ).seconds( ) < static_cast<double>( wait_time ) ) {
return false;
}
return true;
}
175 bool WaitBehaviorTester::behaviorTestCancel(
const float wait_time )
{
if ( !is_active_ ) {
RCLCPP_ERROR( node_->get_logger( ), "Not activated" );
return false;
}
// Sleep to let behavior server be ready for serving in multiple runs
std::this_thread::sleep_for( 5s );
auto start_time = node_->now( );
auto goal_msg = Wait::Goal( );
goal_msg.time = rclcpp::Duration( wait_time, 0.0 );
RCLCPP_INFO( this->node_->get_logger( ), "Sending goal" );
auto goal_handle_future = client_ptr_->async_send_goal( goal_msg );
if ( rclcpp::spin_until_future_complete( node_, goal_handle_future ) !=
rclcpp::FutureReturnCode::SUCCESS )
{
RCLCPP_ERROR( node_->get_logger( ), "send goal call failed :( " );
return false;
}
rclcpp_action::ClientGoalHandle<Wait>::SharedPtr goal_handle = goal_handle_future.get( );
if ( !goal_handle ) {
RCLCPP_ERROR( node_->get_logger( ), "Goal was rejected by server" );
return false;
}
// Wait for the server to be done with the goal
auto result_future = client_ptr_->async_cancel_all_goals( );
RCLCPP_INFO( node_->get_logger( ), "Waiting for cancellation" );
if ( rclcpp::spin_until_future_complete( node_, result_future ) !=
rclcpp::FutureReturnCode::SUCCESS )
{
RCLCPP_ERROR( node_->get_logger( ), "get cancel result call failed :( " );
return false;
}
auto status = goal_handle_future.get( )->get_status( );
switch ( status ) {
case rclcpp_action::GoalStatus::STATUS_SUCCEEDED: RCLCPP_ERROR(
node_->get_logger( ),
"Goal succeeded" );
return false;
case rclcpp_action::GoalStatus::STATUS_ABORTED: RCLCPP_ERROR(
node_->get_logger( ),
"Goal was aborted" );
return false;
case rclcpp_action::GoalStatus::STATUS_CANCELED: RCLCPP_INFO(
node_->get_logger( ),
"Goal was canceled" );
return true;
case rclcpp_action::GoalStatus::STATUS_CANCELING: RCLCPP_INFO(
node_->get_logger( ),
"Goal is cancelling" );
return true;
case rclcpp_action::GoalStatus::STATUS_EXECUTING: RCLCPP_ERROR(
node_->get_logger( ),
"Goal is executing" );
return false;
case rclcpp_action::GoalStatus::STATUS_ACCEPTED: RCLCPP_ERROR(
node_->get_logger( ),
"Goal is processing" );
return false;
default: RCLCPP_ERROR( node_->get_logger( ), "Unknown result code" );
return false;
}
return false;
}
252 void WaitBehaviorTester::sendInitialPose( )
{
geometry_msgs::msg::PoseWithCovarianceStamped pose;
pose.header.frame_id = "map";
pose.header.stamp = rclcpp::Time( );
pose.pose.pose.position.x = -2.0;
pose.pose.pose.position.y = -0.5;
pose.pose.pose.position.z = 0.0;
pose.pose.pose.orientation.x = 0.0;
pose.pose.pose.orientation.y = 0.0;
pose.pose.pose.orientation.z = 0.0;
pose.pose.pose.orientation.w = 1.0;
for ( int i = 0; i < 35; i++ ) {
pose.pose.covariance[i] = 0.0;
}
pose.pose.covariance[0] = 0.08;
pose.pose.covariance[7] = 0.08;
pose.pose.covariance[35] = 0.05;
publisher_->publish( pose );
RCLCPP_INFO( node_->get_logger( ), "Sent initial pose" );
}
275 void WaitBehaviorTester::amclPoseCallback(
276 const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr )
{
initial_pose_received_ = true;
}
} // namespace nav2_system_tests
// Copyright ( c ) 2020 Samsung Research
// Copyright ( c ) 2020 Sarthak Mittal
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef BEHAVIORS__WAIT__WAIT_BEHAVIOR_TESTER_HPP_
#define BEHAVIORS__WAIT__WAIT_BEHAVIOR_TESTER_HPP_
#include <gtest/gtest.h>
#include <memory>
#include <string>
#include <thread>
#include <algorithm>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "angles/angles.h"
#include "nav2_msgs/action/wait.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav2_util/node_thread.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
#include "tf2/utils.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
namespace nav2_system_tests
{
42 class WaitBehaviorTester
{
public:
using Wait = nav2_msgs::action::Wait;
using GoalHandleWait = rclcpp_action::ClientGoalHandle<Wait>;
WaitBehaviorTester( );
~WaitBehaviorTester( );
// Runs a single test with given target yaw
bool behaviorTest(
float time );
bool behaviorTestCancel( float time );
void activate( );
void deactivate( );
bool isActive( ) const
{
return is_active_;
}
private:
void sendInitialPose( );
69 void amclPoseCallback( geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr );
bool is_active_;
bool initial_pose_received_;
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
rclcpp::Node::SharedPtr node_;
// Publisher to publish initial pose
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr publisher_;
// Subscriber for amcl pose
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
// Action client to call wait action
rclcpp_action::Client<Wait>::SharedPtr client_ptr_;
};
} // namespace nav2_system_tests
#endif // BEHAVIORS__WAIT__WAIT_BEHAVIOR_TESTER_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <chrono>
#include <ctime>
#include <thread>
#include <memory>
#include <utility>
#include "dummy_controller.hpp"
using namespace std::chrono_literals;
namespace nav2_system_tests
{
28 DummyController::DummyController( )
: Node( "DummyController" )
{
RCLCPP_INFO( get_logger( ), "Initializing DummyController..." );
auto temp_node = std::shared_ptr<rclcpp::Node>( this, []( auto ) {} );
vel_pub_ =
this->create_publisher<geometry_msgs::msg::Twist>( "cmd_vel", 1 );
task_server_ = std::make_unique<nav2_behavior_tree::FollowPathTaskServer>( temp_node, false ),
task_server_->setExecuteCallback(
std::bind( &DummyController::followPath, this, std::placeholders::_1 ) );
// Start listening for incoming ComputePathToPose action server requests
task_server_->start( );
RCLCPP_INFO( get_logger( ), "Initialized DummyController" );
}
48 DummyController::~DummyController( )
{
RCLCPP_INFO( get_logger( ), "Shutting down DummyController" );
}
void
54 DummyController::followPath( const nav2_behavior_tree::FollowPathCommand::SharedPtr /*command*/ )
{
RCLCPP_INFO( get_logger( ), "Starting controller " );
auto start_time = std::chrono::system_clock::now( );
auto time_since_msg = std::chrono::system_clock::now( );
while ( true ) {
// Dummy controller computation time
std::this_thread::sleep_for( 50ms );
if ( task_server_->cancelRequested( ) ) {
RCLCPP_INFO( get_logger( ), "Task cancelled" );
setZeroVelocity( );
task_server_->setCanceled( );
return;
}
// Log a message every second
auto current_time = std::chrono::system_clock::now( );
if ( current_time - time_since_msg >= 1s ) {
RCLCPP_INFO( get_logger( ), "Following path" );
time_since_msg = std::chrono::system_clock::now( );
}
// Output control command
auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>( );
cmd_vel->linear.x = 0.1;
vel_pub_->publish( std::move( cmd_vel ) );
if ( current_time - start_time >= 30s ) {
RCLCPP_INFO( get_logger( ), "Reached end point" );
setZeroVelocity( );
break;
}
}
nav2_behavior_tree::FollowPathResult result;
task_server_->setResult( result );
}
95 void DummyController::setZeroVelocity( )
{
auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>( );
cmd_vel->linear.x = 0.0;
cmd_vel->linear.y = 0.0;
cmd_vel->angular.z = 0.0;
vel_pub_->publish( std::move( cmd_vel ) );
}
} // namespace nav2_system_tests
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef DUMMY_CONTROLLER__DUMMY_CONTROLLER_HPP_
#define DUMMY_CONTROLLER__DUMMY_CONTROLLER_HPP_
#include <memory>
#include "nav2_behavior_tree/follow_path_task.hpp"
#include "geometry_msgs/msg/twist.hpp"
namespace nav2_system_tests
{
26 class DummyController : public rclcpp::Node
{
public:
29 DummyController( );
30 ~DummyController( );
32 nav2_behavior_tree::TaskStatus followPath(
33 const nav2_behavior_tree::FollowPathCommand::SharedPtr command );
private:
36 void setZeroVelocity( );
38 std::unique_ptr<nav2_behavior_tree::FollowPathTaskServer> task_server_;
std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::Twist>> vel_pub_;
};
} // namespace nav2_system_tests
#endif // DUMMY_CONTROLLER__DUMMY_CONTROLLER_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "dummy_controller.hpp"
19 int main( int argc, char ** argv )
{
rclcpp::init( argc, argv );
rclcpp::spin( std::make_shared<nav2_system_tests::DummyController>( ) );
rclcpp::shutdown( );
return 0;
}
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <chrono>
#include <thread>
#include <memory>
#include "dummy_planner.hpp"
using namespace std::chrono_literals;
namespace nav2_system_tests
{
26 DummyPlanner::DummyPlanner( )
: Node( "DummyPlanner" )
{
RCLCPP_INFO( get_logger( ), "Initializing DummyPlanner..." );
auto temp_node = std::shared_ptr<rclcpp::Node>( this, []( auto ) {} );
task_server_ =
std::make_unique<nav2_behavior_tree::ComputePathToPoseTaskServer>( temp_node, false ),
task_server_->setExecuteCallback(
std::bind( &DummyPlanner::computePlan, this, std::placeholders::_1 ) );
// Start listening for incoming ComputePathToPose task requests
task_server_->start( );
RCLCPP_INFO( get_logger( ), "Initialized DummyPlanner" );
}
44 DummyPlanner::~DummyPlanner( )
{
RCLCPP_INFO( get_logger( ), "Shutting down DummyPlanner" );
}
void
50 DummyPlanner::computePlan( const nav2_behavior_tree::ComputePathToPoseCommand::SharedPtr cmd )
{
RCLCPP_INFO(
get_logger( ), "Attempting to a find path from ( %.2f, %.2f ) to "
"( %.2f, %.2f ).", cmd->start.position.x, cmd->start.position.y,
cmd->goal.position.x, cmd->goal.position.y );
// Dummy path computation time
std::this_thread::sleep_for( 500ms );
if ( task_server_->cancelRequested( ) ) {
RCLCPP_INFO( get_logger( ), "Cancelled planning task." );
task_server_->setCanceled( );
return;
}
RCLCPP_INFO( get_logger( ), "Found a dummy path" );
nav2_behavior_tree::ComputePathToPoseResult result;
// set succeeded
task_server_->setResult( result );
}
} // namespace nav2_system_tests
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef DUMMY_PLANNER__DUMMY_PLANNER_HPP_
#define DUMMY_PLANNER__DUMMY_PLANNER_HPP_
#include <memory>
#include "nav2_behavior_tree/compute_path_to_pose_task.hpp"
namespace nav2_system_tests
{
25 class DummyPlanner : public rclcpp::Node
{
public:
28 DummyPlanner( );
29 ~DummyPlanner( );
31 nav2_behavior_tree::TaskStatus computePathToPose(
32 const nav2_behavior_tree::ComputePathToPoseCommand::SharedPtr command );
private:
35 std::unique_ptr<nav2_behavior_tree::ComputePathToPoseTaskServer> task_server_;
};
} // namespace nav2_system_tests
#endif // DUMMY_PLANNER__DUMMY_PLANNER_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "dummy_planner.hpp"
19 int main( int argc, char ** argv )
{
rclcpp::init( argc, argv );
rclcpp::spin( std::make_shared<nav2_system_tests::DummyPlanner>( ) );
rclcpp::shutdown( );
return 0;
}
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_amcl/amcl_node.hpp"
#include "std_msgs/msg/string.hpp"
#include "geometry_msgs/msg/pose_array.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
using std::placeholders::_1;
using namespace std::chrono_literals;
// rclcpp::init can only be called once per process, so this needs to be a global variable
27 class RclCppFixture
{
public:
30 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
31 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
35 class TestAmclPose : public ::testing::Test
{
public:
38 TestAmclPose( )
{
pose_callback_ = false;
initTestPose( );
tol_ = 0.25;
node = rclcpp::Node::make_shared( "localization_test" );
while ( node->count_subscribers( "scan" ) < 1 ) {
std::this_thread::sleep_for( 100ms );
rclcpp::spin_some( node );
}
initial_pose_pub_ = node->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
"initialpose", rclcpp::SystemDefaultsQoS( ) );
subscription_ = node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
"amcl_pose", rclcpp::QoS( rclcpp::KeepLast( 1 ) ).transient_local( ).reliable( ),
std::bind( &TestAmclPose::amcl_pose_callback, this, _1 ) );
initial_pose_pub_->publish( testPose_ );
}
59 bool defaultAmclTest( );
protected:
62 std::shared_ptr<rclcpp::Node> node;
63 void initTestPose( );
private:
66 void amcl_pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg )
{
auto amcl_pose = msg->pose;
amcl_pose_x = amcl_pose.pose.position.x;
amcl_pose_y = amcl_pose.pose.position.y;
pose_callback_ = true;
}
73 rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_pub_;
74 rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
75 geometry_msgs::msg::PoseWithCovarianceStamped testPose_;
double amcl_pose_x;
double amcl_pose_y;
78 bool pose_callback_;
float tol_;
};
82 bool TestAmclPose::defaultAmclTest( )
{
initial_pose_pub_->publish( testPose_ );
while ( !pose_callback_ ) {
// TODO( mhpanah ): Initial pose should only be published once.
initial_pose_pub_->publish( testPose_ );
std::this_thread::sleep_for( 1s );
rclcpp::spin_some( node );
}
if ( std::abs( amcl_pose_x - testPose_.pose.pose.position.x ) < tol_ &&
std::abs( amcl_pose_y - testPose_.pose.pose.position.y ) < tol_ )
{
return true;
} else {
return false;
}
}
100 void TestAmclPose::initTestPose( )
{
testPose_.header.frame_id = "map";
testPose_.header.stamp = rclcpp::Time( );
testPose_.pose.pose.position.x = -2.0;
testPose_.pose.pose.position.y = -0.5;
testPose_.pose.pose.position.z = 0.0;
testPose_.pose.pose.orientation.x = 0.0;
testPose_.pose.pose.orientation.y = 0.0;
testPose_.pose.pose.orientation.z = 0.0;
testPose_.pose.pose.orientation.w = 1.0;
for ( int i = 0; i < 35; i++ ) {
testPose_.pose.covariance[i] = 0.0;
}
testPose_.pose.covariance[0] = 0.08;
testPose_.pose.covariance[7] = 0.08;
testPose_.pose.covariance[35] = 0.05;
}
119 TEST_F( TestAmclPose, SimpleAmclTest )
{
EXPECT_EQ( true, defaultAmclTest( ) );
}
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <string>
#include <random>
#include <tuple>
#include <utility>
#include <vector>
#include <memory>
#include <iostream>
#include <chrono>
#include <sstream>
#include <iomanip>
#include "planner_tester.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "nav2_map_server/map_mode.hpp"
#include "nav2_map_server/map_io.hpp"
#include "nav2_msgs/msg/costmap_meta_data.hpp"
using namespace std::chrono_literals;
using namespace std::chrono; // NOLINT
using nav2_util::Costmap;
using nav2_util::TestCostmap;
namespace nav2_system_tests
{
40 PlannerTester::PlannerTester( )
: Node( "PlannerTester" ), is_active_( false ),
map_set_( false ), costmap_set_( false ),
using_fake_costmap_( true ), trinary_costmap_( true ),
track_unknown_space_( false ), lethal_threshold_( 100 ), unknown_cost_value_( -1 ),
testCostmapType_( TestCostmap::open_space ), base_transform_( nullptr ),
map_publish_rate_( 100s )
{
}
50 void PlannerTester::activate( )
{
if ( is_active_ ) {
throw std::runtime_error( "Trying to activate while already active" );
return;
}
is_active_ = true;
// Launch a thread to process the messages for this node
spin_thread_ = std::make_unique<nav2_util::NodeThread>( this );
// We start with a 10x10 grid with no obstacles
costmap_ = std::make_unique<Costmap>( this );
loadSimpleCostmap( TestCostmap::open_space );
startRobotTransform( );
// The navfn wrapper
auto state = rclcpp_lifecycle::State( );
planner_tester_ = std::make_shared<NavFnPlannerTester>( );
planner_tester_->declare_parameter(
"GridBased.use_astar", rclcpp::ParameterValue( true ) );
planner_tester_->set_parameter(
rclcpp::Parameter( std::string( "GridBased.use_astar" ), rclcpp::ParameterValue( true ) ) );
planner_tester_->set_parameter(
rclcpp::Parameter( std::string( "expected_planner_frequency" ), rclcpp::ParameterValue( -1.0 ) ) );
planner_tester_->onConfigure( state );
publishRobotTransform( );
map_pub_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>( "map", 1 );
path_valid_client_ = this->create_client<nav2_msgs::srv::IsPathValid>( "is_path_valid" );
rclcpp::Rate r( 1 );
r.sleep( );
planner_tester_->onActivate( state );
}
85 void PlannerTester::deactivate( )
{
if ( !is_active_ ) {
throw std::runtime_error( "Trying to deactivate while already inactive" );
return;
}
is_active_ = false;
spin_thread_.reset( );
auto state = rclcpp_lifecycle::State( );
planner_tester_->onDeactivate( state );
planner_tester_->onCleanup( state );
map_timer_.reset( );
map_pub_.reset( );
map_.reset( );
tf_broadcaster_.reset( );
}
105 PlannerTester::~PlannerTester( )
{
if ( is_active_ ) {
deactivate( );
}
}
112 void PlannerTester::startRobotTransform( )
{
// Provide the robot pose transform
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>( this );
// Set an initial pose
geometry_msgs::msg::Point robot_position;
robot_position.x = 1.0;
robot_position.y = 1.0;
updateRobotPosition( robot_position );
// Publish the transform periodically
transform_timer_ = create_wall_timer(
100ms, std::bind( &PlannerTester::publishRobotTransform, this ) );
}
128 void PlannerTester::updateRobotPosition( const geometry_msgs::msg::Point & position )
{
if ( !base_transform_ ) {
base_transform_ = std::make_unique<geometry_msgs::msg::TransformStamped>( );
base_transform_->header.frame_id = "map";
base_transform_->child_frame_id = "base_link";
}
std::cout << now( ).nanoseconds( ) << std::endl;
base_transform_->header.stamp = now( ) + rclcpp::Duration( 0.25s );
base_transform_->transform.translation.x = position.x;
base_transform_->transform.translation.y = position.y;
base_transform_->transform.rotation.w = 1.0;
publishRobotTransform( );
}
145 void PlannerTester::publishRobotTransform( )
{
if ( base_transform_ ) {
tf_broadcaster_->sendTransform( *base_transform_ );
}
}
152 void PlannerTester::loadDefaultMap( )
{
// Specs for the default map
double resolution = 1.0;
bool negate = false;
double occupancy_threshold = 0.65;
double free_threshold = 0.196;
// Define origin offset
std::vector<double> origin = {0.0, 0.0, 0.0};
nav2_map_server::MapMode mode = nav2_map_server::MapMode::Trinary;
std::string file_path = "";
char const * path = getenv( "TEST_MAP" );
if ( path == NULL ) {
throw std::runtime_error(
"Path to map image file"
" has not been specified in environment variable `TEST_MAP`." );
} else {
file_path = std::string( path );
}
RCLCPP_INFO( this->get_logger( ), "Loading map with file_path: %s", file_path.c_str( ) );
try {
map_ = std::make_shared<nav_msgs::msg::OccupancyGrid>( );
nav2_map_server::LoadParameters load_parameters;
load_parameters.image_file_name = file_path;
load_parameters.resolution = resolution;
load_parameters.origin = origin;
load_parameters.free_thresh = free_threshold;
load_parameters.occupied_thresh = occupancy_threshold;
load_parameters.mode = mode;
load_parameters.negate = negate;
loadMapFromFile( load_parameters, *map_ );
} catch ( ... ) {
RCLCPP_ERROR(
this->get_logger( ),
"Failed to load image from file: %s", file_path.c_str( ) );
throw;
}
map_->header.stamp = this->now( );
map_->header.frame_id = "map";
map_->info.map_load_time = this->now( );
// TODO( orduno ): #443 replace with a latched topic
map_timer_ = create_wall_timer( 1s, [this]( ) -> void {map_pub_->publish( *map_ );} );
map_set_ = true;
costmap_set_ = false;
using_fake_costmap_ = false;
setCostmap( );
}
210 void PlannerTester::loadSimpleCostmap( const TestCostmap & testCostmapType )
{
RCLCPP_INFO( get_logger( ), "loadSimpleCostmap called." );
if ( costmap_set_ ) {
RCLCPP_DEBUG( this->get_logger( ), "Setting a new costmap with fake values" );
}
costmap_->set_test_costmap( testCostmapType );
costmap_set_ = true;
using_fake_costmap_ = true;
}
223 void PlannerTester::setCostmap( )
{
if ( !map_set_ ) {
RCLCPP_ERROR( this->get_logger( ), "Map has not been provided" );
return;
}
costmap_ = std::make_unique<Costmap>(
this, trinary_costmap_, track_unknown_space_, lethal_threshold_, unknown_cost_value_ );
costmap_->set_static_map( *map_ );
costmap_set_ = true;
using_fake_costmap_ = false;
}
239 bool PlannerTester::defaultPlannerTest(
240 ComputePathToPoseResult & path,
const double /*deviation_tolerance*/ )
{
if ( !costmap_set_ ) {
RCLCPP_ERROR( this->get_logger( ), "Costmap must be set before requesting a plan" );
return false;
}
// TODO( orduno ) #443 Add support for planners that take into account robot orientation
geometry_msgs::msg::Point robot_position;
ComputePathToPoseCommand goal;
auto costmap_properties = costmap_->get_properties( );
if ( using_fake_costmap_ ) {
RCLCPP_DEBUG( this->get_logger( ), "Planning using a fake costmap" );
robot_position.x = 1.0;
robot_position.y = 1.0;
goal.pose.position.x = 8.0;
goal.pose.position.y = 8.0;
} else {
RCLCPP_DEBUG( this->get_logger( ), "Planning using the provided map" );
// Defined with respect to world coordinate system
// Planner will do coordinate transformation to map internally
robot_position.x = 390.0;
robot_position.y = 10.0;
goal.pose.position.x = 10.0;
goal.pose.position.y = 390.0;
}
// TODO( orduno ): #443 On a default test, provide the reference path to compare with the planner
// result.
return plannerTest( robot_position, goal, path );
}
279 bool PlannerTester::defaultPlannerRandomTests(
const unsigned int number_tests,
const float acceptable_fail_ratio = 0.1 )
{
if ( !costmap_set_ ) {
RCLCPP_ERROR( this->get_logger( ), "Costmap must be set before requesting a plan" );
return false;
}
if ( using_fake_costmap_ ) {
RCLCPP_ERROR(
this->get_logger( ),
"Randomized testing with hardcoded costmaps not implemented yet" );
return false;
}
// Initialize random number generator
std::random_device random_device;
std::mt19937 generator( random_device( ) );
// Obtain random positions within map
std::uniform_int_distribution<> distribution_x( 1, costmap_->get_properties( ).size_x - 1 );
std::uniform_int_distribution<> distribution_y( 1, costmap_->get_properties( ).size_y - 1 );
auto generate_random = [&]( ) mutable -> std::pair<int, int> {
bool point_is_free = false;
int x, y;
while ( !point_is_free ) {
x = distribution_x( generator );
y = distribution_y( generator );
point_is_free = costmap_->is_free( x, y );
}
return std::make_pair( x, y );
};
// TODO( orduno ) #443 Add support for planners that take into account robot orientation
geometry_msgs::msg::Point robot_position;
ComputePathToPoseCommand goal;
ComputePathToPoseResult path;
unsigned int num_fail = 0;
auto start = high_resolution_clock::now( );
for ( unsigned int test_num = 0; test_num < number_tests; ++test_num ) {
RCLCPP_DEBUG( this->get_logger( ), "Running test #%u", test_num + 1 );
// Compose the robot start position and goal using random numbers
// Defined with respect to world coordinate system
// Planner will do coordinate transformation to map internally
auto vals = generate_random( );
robot_position.x = vals.first;
robot_position.y = vals.second;
vals = generate_random( );
goal.pose.position.x = vals.first;
goal.pose.position.y = vals.second;
if ( !plannerTest( robot_position, goal, path ) ) {
RCLCPP_WARN(
this->get_logger( ), "Failed with start at %0.2f, %0.2f and goal at %0.2f, %0.2f",
robot_position.x, robot_position.y, goal.pose.position.x, goal.pose.position.y );
++num_fail;
}
}
auto end = high_resolution_clock::now( );
auto elapsed = duration_cast<milliseconds>( end - start );
RCLCPP_INFO(
this->get_logger( ),
"Tested with %u tests. Planner failed on %u. Test time %ld ms",
number_tests, num_fail, elapsed.count( ) );
if ( ( num_fail / number_tests ) > acceptable_fail_ratio ) {
return false;
}
return true;
}
358 bool PlannerTester::plannerTest(
359 const geometry_msgs::msg::Point & robot_position,
360 const ComputePathToPoseCommand & goal,
361 ComputePathToPoseResult & path )
{
RCLCPP_DEBUG( this->get_logger( ), "Getting the path from the planner" );
// First make available the current robot position for the planner to take as starting point
updateRobotPosition( robot_position );
sleep( 0.05 );
// Then request to compute a path
TaskStatus status = createPlan( goal, path );
RCLCPP_DEBUG( this->get_logger( ), "Path request status: %d", static_cast<int8_t>( status ) );
if ( status == TaskStatus::FAILED ) {
return false;
} else if ( status == TaskStatus::SUCCEEDED ) {
// TODO( orduno ): #443 check why task may report success while planner returns a path of 0 points
RCLCPP_DEBUG( this->get_logger( ), "Got path, checking for possible collisions" );
return isCollisionFree( path ) && isWithinTolerance( robot_position, goal, path );
}
return false;
}
385 TaskStatus PlannerTester::createPlan(
386 const ComputePathToPoseCommand & goal,
387 ComputePathToPoseResult & path )
{
// Update the costmap of the planner to the set data
planner_tester_->setCostmap( costmap_.get( ) );
// Call planning algorithm
if ( planner_tester_->createPath( goal, path ) ) {
return TaskStatus::SUCCEEDED;
}
return TaskStatus::FAILED;
}
400 bool PlannerTester::isPathValid( nav_msgs::msg::Path & path )
{
planner_tester_->setCostmap( costmap_.get( ) );
// create a fake service request
auto request = std::make_shared<nav2_msgs::srv::IsPathValid::Request>( );
request->path = path;
auto result = path_valid_client_->async_send_request( request );
RCLCPP_INFO( this->get_logger( ), "Waiting for service complete" );
if ( rclcpp::spin_until_future_complete(
this->planner_tester_, result,
std::chrono::milliseconds( 100 ) ) ==
rclcpp::FutureReturnCode::SUCCESS )
{
return result.get( )->is_valid;
} else {
RCLCPP_INFO( get_logger( ), "Failed to call is_path_valid service" );
return false;
}
}
421 bool PlannerTester::isCollisionFree( const ComputePathToPoseResult & path )
{
// At each point of the path, check if the corresponding cell is free
// TODO( orduno ): #443 for now we are assuming the robot is the size of a single cell
// costmap/world_model has consider the robot footprint
// TODO( orduno ): #443 Tweak criteria for defining if a path goes into obstacles.
// Current navfn planner will sometimes produce paths that cut corners
// i.e. some points are around the corner are actually inside the obstacle
bool collisionFree = true;
for ( auto pose : path.poses ) {
collisionFree = costmap_->is_free(
static_cast<unsigned int>( std::round( pose.pose.position.x ) ),
static_cast<unsigned int>( std::round( pose.pose.position.y ) ) );
if ( !collisionFree ) {
RCLCPP_WARN(
this->get_logger( ), "Path has collision at ( %.2f, %.2f )",
pose.pose.position.x, pose.pose.position.y );
printPath( path );
return false;
}
}
RCLCPP_DEBUG( this->get_logger( ), "Path has no collisions" );
return true;
}
452 bool PlannerTester::isWithinTolerance(
453 const geometry_msgs::msg::Point & robot_position,
454 const ComputePathToPoseCommand & goal,
455 const ComputePathToPoseResult & path ) const
{
return isWithinTolerance(
robot_position, goal, path, 0.0, ComputePathToPoseResult( ) );
}
461 bool PlannerTester::isWithinTolerance(
462 const geometry_msgs::msg::Point & robot_position,
463 const ComputePathToPoseCommand & goal,
464 const ComputePathToPoseResult & path,
const double /*deviationTolerance*/,
466 const ComputePathToPoseResult & /*reference_path*/ ) const
{
// TODO( orduno ) #443 Work in progress, for now we only check that the path start matches the
// robot start location and that the path end matches the goal.
auto path_start = path.poses[0];
auto path_end = path.poses.end( )[-1];
if (
path_start.pose.position.x == robot_position.x &&
path_start.pose.position.y == robot_position.y &&
path_end.pose.position.x == goal.pose.position.x &&
path_end.pose.position.y == goal.pose.position.y )
{
RCLCPP_DEBUG( this->get_logger( ), "Path has correct start and end points" );
return true;
}
RCLCPP_WARN( this->get_logger( ), "Path deviates from requested start and end points" );
RCLCPP_DEBUG(
this->get_logger( ), "Requested path starts at ( %.2f, %.2f ) and ends at ( %.2f, %.2f )",
robot_position.x, robot_position.y, goal.pose.position.x, goal.pose.position.y );
RCLCPP_DEBUG(
this->get_logger( ), "Computed path starts at ( %.2f, %.2f ) and ends at ( %.2f, %.2f )",
path_start.pose.position.x, path_start.pose.position.y,
path_end.pose.position.x, path_end.pose.position.y );
return false;
}
498 void PlannerTester::printPath( const ComputePathToPoseResult & path ) const
{
auto index = 0;
auto ss = std::stringstream{};
for ( auto pose : path.poses ) {
ss << " point #" << index << " with" <<
" x: " << std::setprecision( 3 ) << pose.pose.position.x <<
" y: " << std::setprecision( 3 ) << pose.pose.position.y << '\n';
++index;
}
RCLCPP_INFO( get_logger( ), ss.str( ).c_str( ) );
}
} // namespace nav2_system_tests
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#ifndef PLANNING__PLANNER_TESTER_HPP_
#define PLANNING__PLANNER_TESTER_HPP_
#include <gtest/gtest.h>
#include <memory>
#include <string>
#include <thread>
#include <algorithm>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_msgs/action/compute_path_to_pose.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
#include "nav2_msgs/msg/costmap.hpp"
#include "nav2_msgs/srv/get_costmap.hpp"
#include "nav2_msgs/srv/is_path_valid.hpp"
#include "visualization_msgs/msg/marker.hpp"
#include "nav2_util/costmap.hpp"
#include "nav2_util/node_thread.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2_msgs/msg/tf_message.hpp"
#include "nav2_planner/planner_server.hpp"
#include "tf2_ros/transform_broadcaster.h"
namespace nav2_system_tests
{
43 class NavFnPlannerTester : public nav2_planner::PlannerServer
{
public:
46 NavFnPlannerTester( )
: PlannerServer( )
{
}
51 void printCostmap( )
{
// print costmap for debug
for ( size_t i = 0; i != costmap_->getSizeInCellsX( ) * costmap_->getSizeInCellsY( ); i++ ) {
if ( i % costmap_->getSizeInCellsX( ) == 0 ) {
std::cout << "" << std::endl;
}
std::cout << costmap_ros_->getCostmap( )->getCharMap( )[i] << " ";
}
std::cout << "" << std::endl;
}
63 void setCostmap( nav2_util::Costmap * costmap )
{
nav2_msgs::msg::CostmapMetaData prop;
nav2_msgs::msg::Costmap cm = costmap->get_costmap( prop );
prop = cm.metadata;
costmap_ros_->getCostmap( )->resizeMap(
prop.size_x, prop.size_y,
prop.resolution, prop.origin.position.x, prop.origin.position.x );
// Volatile prevents compiler from treating costmap_ptr as unused or changing its address
volatile unsigned char * costmap_ptr = costmap_ros_->getCostmap( )->getCharMap( );
delete[] costmap_ptr;
costmap_ptr = new unsigned char[prop.size_x * prop.size_y];
std::copy( cm.data.begin( ), cm.data.end( ), costmap_ptr );
}
78 bool createPath(
79 const geometry_msgs::msg::PoseStamped & goal,
80 nav_msgs::msg::Path & path )
{
geometry_msgs::msg::PoseStamped start;
if ( !nav2_util::getCurrentPose( start, *tf_, "map", "base_link", 0.1 ) ) {
return false;
}
try {
path = planners_["GridBased"]->createPlan( start, goal );
// The situation when createPlan( ) did not throw any exception
// does not guarantee that plan was created correctly.
// So it should be checked additionally that path is correct.
if ( !path.poses.size( ) ) {
return false;
}
} catch ( ... ) {
return false;
}
return true;
}
100 void onCleanup( const rclcpp_lifecycle::State & state )
{
on_cleanup( state );
}
105 void onActivate( const rclcpp_lifecycle::State & state )
{
on_activate( state );
}
110 void onDeactivate( const rclcpp_lifecycle::State & state )
{
on_deactivate( state );
}
115 void onConfigure( const rclcpp_lifecycle::State & state )
{
on_configure( state );
}
};
121 enum class TaskStatus : int8_t
{
SUCCEEDED = 1,
FAILED = 2,
RUNNING = 3,
};
128 class PlannerTester : public rclcpp::Node
{
public:
using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped;
using ComputePathToPoseResult = nav_msgs::msg::Path;
PlannerTester( );
~PlannerTester( );
// Activate the tester before running tests
void activate( );
void deactivate( );
// Loads the provided map and and generates a costmap from it.
void loadDefaultMap( );
// Alternatively, use a preloaded 10x10 costmap
void loadSimpleCostmap( const nav2_util::TestCostmap & testCostmapType );
// Runs a single test with default poses depending on the loaded map
// Success criteria is a collision free path and a deviation to a
// reference path smaller than a tolerance.
bool defaultPlannerTest(
ComputePathToPoseResult & path,
const double deviation_tolerance = 1.0 );
// Runs multiple tests with random initial and goal poses
bool defaultPlannerRandomTests(
const unsigned int number_tests,
const float acceptable_fail_ratio );
bool isPathValid( nav_msgs::msg::Path & path );
private:
void setCostmap( );
TaskStatus createPlan(
const ComputePathToPoseCommand & goal,
ComputePathToPoseResult & path
);
bool is_active_;
bool map_set_;
bool costmap_set_;
bool using_fake_costmap_;
// Parameters of the costmap
bool trinary_costmap_;
bool track_unknown_space_;
int lethal_threshold_;
int unknown_cost_value_;
nav2_util::TestCostmap testCostmapType_;
// The static map
std::shared_ptr<nav_msgs::msg::OccupancyGrid> map_;
// The costmap representation of the static map
std::unique_ptr<nav2_util::Costmap> costmap_;
// The global planner
std::shared_ptr<NavFnPlannerTester> planner_tester_;
// The is path valid client
rclcpp::Client<nav2_msgs::srv::IsPathValid>::SharedPtr path_valid_client_;
// A thread for spinning the ROS node
std::unique_ptr<nav2_util::NodeThread> spin_thread_;
// The tester must provide the robot pose through a transform
std::unique_ptr<geometry_msgs::msg::TransformStamped> base_transform_;
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
rclcpp::TimerBase::SharedPtr transform_timer_;
void publishRobotTransform( );
void startRobotTransform( );
void updateRobotPosition( const geometry_msgs::msg::Point & position );
// Occupancy grid publisher for visualization
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_pub_;
rclcpp::TimerBase::SharedPtr map_timer_;
rclcpp::WallRate map_publish_rate_;
void mapCallback( );
// Executes a test run with the provided end points.
// Success criteria is a collision free path.
// TODO( orduno ): #443 Assuming a robot the size of a costmap cell
bool plannerTest(
const geometry_msgs::msg::Point & robot_position,
const ComputePathToPoseCommand & goal,
ComputePathToPoseResult & path );
bool isCollisionFree( const ComputePathToPoseResult & path );
bool isWithinTolerance(
const geometry_msgs::msg::Point & robot_position,
const ComputePathToPoseCommand & goal,
const ComputePathToPoseResult & path ) const;
bool isWithinTolerance(
const geometry_msgs::msg::Point & robot_position,
const ComputePathToPoseCommand & goal,
const ComputePathToPoseResult & path,
const double deviationTolerance,
const ComputePathToPoseResult & reference_path ) const;
void printPath( const ComputePathToPoseResult & path ) const;
};
} // namespace nav2_system_tests
#endif // PLANNING__PLANNER_TESTER_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <gtest/gtest.h>
#include <memory>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "planner_tester.hpp"
#include "nav2_util/lifecycle_utils.hpp"
using namespace std::chrono_literals;
using nav2_system_tests::PlannerTester;
using nav2_util::TestCostmap;
using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped;
using ComputePathToPoseResult = nav_msgs::msg::Path;
31 TEST( testSimpleCostmaps, testSimpleCostmaps )
{
auto obj = std::make_shared<PlannerTester>( );
std::vector<TestCostmap> costmaps = {
TestCostmap::open_space,
TestCostmap::bounded,
TestCostmap::top_left_obstacle,
TestCostmap::bottom_left_obstacle,
TestCostmap::maze1,
TestCostmap::maze2
};
ComputePathToPoseResult result;
obj->activate( );
for ( auto costmap : costmaps ) {
obj->loadSimpleCostmap( costmap );
EXPECT_EQ( true, obj->defaultPlannerTest( result ) );
}
}
54 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2022 Joshua Wallace
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <gtest/gtest.h>
#include <memory>
#include <vector>
#include "nav2_msgs/srv/is_path_valid.hpp"
#include "rclcpp/rclcpp.hpp"
#include "planner_tester.hpp"
#include "nav2_util/lifecycle_utils.hpp"
using nav2_system_tests::PlannerTester;
using nav2_util::TestCostmap;
27 TEST( testIsPathValid, testIsPathValid )
{
auto planner_tester = std::make_shared<PlannerTester>( );
planner_tester->activate( );
planner_tester->loadSimpleCostmap( TestCostmap::top_left_obstacle );
nav_msgs::msg::Path path;
// empty path
bool is_path_valid = planner_tester->isPathValid( path );
EXPECT_FALSE( is_path_valid );
// invalid path
for ( float i = 0; i < 10; i += 1.0 ) {
for ( float j = 0; j < 10; j += 1.0 ) {
geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x = i;
pose.pose.position.y = j;
path.poses.push_back( pose );
}
}
is_path_valid = planner_tester->isPathValid( path );
EXPECT_FALSE( is_path_valid );
// valid path
path.poses.clear( );
for ( float i = 0; i < 10; i += 1.0 ) {
geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x = 1.0;
pose.pose.position.y = i;
path.poses.push_back( pose );
}
is_path_valid = planner_tester->isPathValid( path );
EXPECT_TRUE( is_path_valid );
}
63 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2020 Samsung Research
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <gtest/gtest.h>
#include <memory>
#include <vector>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "planner_tester.hpp"
#include "nav2_util/lifecycle_utils.hpp"
#include "nav2_util/geometry_utils.hpp"
using namespace std::chrono_literals;
using nav2_system_tests::PlannerTester;
using nav2_util::TestCostmap;
using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped;
using ComputePathToPoseResult = nav_msgs::msg::Path;
33 void callback( const nav_msgs::msg::Path::ConstSharedPtr /*grid*/ )
{
}
37 void testSmallPathValidityAndOrientation( std::string plugin, double length )
{
auto obj = std::make_shared<nav2_system_tests::NavFnPlannerTester>( );
rclcpp_lifecycle::State state;
obj->set_parameter( rclcpp::Parameter( "GridBased.plugin", plugin ) );
obj->declare_parameter(
"GridBased.use_final_approach_orientation", rclcpp::ParameterValue( false ) );
obj->onConfigure( state );
geometry_msgs::msg::PoseStamped start;
geometry_msgs::msg::PoseStamped goal;
start.pose.position.x = 0.5;
start.pose.position.y = 0.5;
start.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( M_PI_2 );
start.header.frame_id = "map";
goal.pose.position.x = 0.5;
goal.pose.position.y = start.pose.position.y + length;
goal.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( -M_PI );
goal.header.frame_id = "map";
// Test without use_final_approach_orientation
// expecting end path pose orientation to be equal to goal orientation
auto path = obj->getPlan( start, goal, "GridBased" );
EXPECT_GT( ( int )path.poses.size( ), 0 );
EXPECT_NEAR( tf2::getYaw( path.poses.back( ).pose.orientation ), -M_PI, 0.01 );
// obj->onCleanup( state );
obj.reset( );
}
68 void testSmallPathValidityAndNoOrientation( std::string plugin, double length )
{
auto obj = std::make_shared<nav2_system_tests::NavFnPlannerTester>( );
rclcpp_lifecycle::State state;
obj->set_parameter( rclcpp::Parameter( "GridBased.plugin", plugin ) );
// Test WITH use_final_approach_orientation
// expecting end path pose orientation to be equal to approach orientation
// which in the one pose corner case should be the start pose orientation
obj->declare_parameter(
"GridBased.use_final_approach_orientation", rclcpp::ParameterValue( true ) );
obj->set_parameter( rclcpp::Parameter( "GridBased.use_final_approach_orientation", true ) );
obj->onConfigure( state );
geometry_msgs::msg::PoseStamped start;
geometry_msgs::msg::PoseStamped goal;
start.pose.position.x = 0.5;
start.pose.position.y = 0.5;
start.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( M_PI_2 );
start.header.frame_id = "map";
goal.pose.position.x = 0.5;
goal.pose.position.y = start.pose.position.y + length;
goal.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis( -M_PI );
goal.header.frame_id = "map";
auto path = obj->getPlan( start, goal, "GridBased" );
EXPECT_GT( ( int )path.poses.size( ), 0 );
int path_size = path.poses.size( );
if ( path_size == 1 ) {
EXPECT_NEAR(
tf2::getYaw( path.poses.back( ).pose.orientation ),
tf2::getYaw( start.pose.orientation ),
0.01 );
} else {
double dx = path.poses.back( ).pose.position.x - path.poses.front( ).pose.position.x;
double dy = path.poses.back( ).pose.position.y - path.poses.front( ).pose.position.y;
EXPECT_NEAR(
tf2::getYaw( path.poses.back( ).pose.orientation ),
atan2( dy, dx ),
0.01 );
}
// obj->onCleanup( state );
obj.reset( );
}
116 TEST( testPluginMap, Failures )
{
auto obj = std::make_shared<nav2_system_tests::NavFnPlannerTester>( );
rclcpp_lifecycle::State state;
obj->set_parameter( rclcpp::Parameter( "expected_planner_frequency", 100000.0 ) );
obj->onConfigure( state );
obj->create_subscription<nav_msgs::msg::Path>(
"plan", rclcpp::SystemDefaultsQoS( ), callback );
geometry_msgs::msg::PoseStamped start;
geometry_msgs::msg::PoseStamped goal;
std::string plugin_fake = "fake";
std::string plugin_none = "";
auto path = obj->getPlan( start, goal, plugin_none );
EXPECT_EQ( path.header.frame_id, std::string( "map" ) );
path = obj->getPlan( start, goal, plugin_fake );
EXPECT_EQ( path.poses.size( ), 0ul );
obj->onCleanup( state );
}
138 TEST( testPluginMap, Smac2dEqualStartGoal )
{
testSmallPathValidityAndOrientation( "nav2_smac_planner/SmacPlanner2D", 0.0 );
}
143 TEST( testPluginMap, Smac2dEqualStartGoalN )
{
testSmallPathValidityAndNoOrientation( "nav2_smac_planner/SmacPlanner2D", 0.0 );
}
148 TEST( testPluginMap, Smac2dVerySmallPath )
{
testSmallPathValidityAndOrientation( "nav2_smac_planner/SmacPlanner2D", 0.00001 );
}
153 TEST( testPluginMap, Smac2dVerySmallPathN )
{
testSmallPathValidityAndNoOrientation( "nav2_smac_planner/SmacPlanner2D", 0.00001 );
}
158 TEST( testPluginMap, Smac2dBelowCostmapResolution )
{
testSmallPathValidityAndOrientation( "nav2_smac_planner/SmacPlanner2D", 0.09 );
}
163 TEST( testPluginMap, Smac2dBelowCostmapResolutionN )
{
testSmallPathValidityAndNoOrientation( "nav2_smac_planner/SmacPlanner2D", 0.09 );
}
168 TEST( testPluginMap, Smac2dJustAboveCostmapResolution )
{
testSmallPathValidityAndOrientation( "nav2_smac_planner/SmacPlanner2D", 0.102 );
}
173 TEST( testPluginMap, Smac2dJustAboveCostmapResolutionN )
{
testSmallPathValidityAndNoOrientation( "nav2_smac_planner/SmacPlanner2D", 0.102 );
}
178 TEST( testPluginMap, Smac2dAboveCostmapResolution )
{
testSmallPathValidityAndOrientation( "nav2_smac_planner/SmacPlanner2D", 1.5 );
}
183 TEST( testPluginMap, Smac2dAboveCostmapResolutionN )
{
testSmallPathValidityAndNoOrientation( "nav2_smac_planner/SmacPlanner2D", 1.5 );
}
188 TEST( testPluginMap, NavFnEqualStartGoal )
{
testSmallPathValidityAndOrientation( "nav2_navfn_planner/NavfnPlanner", 0.0 );
}
193 TEST( testPluginMap, NavFnEqualStartGoalN )
{
testSmallPathValidityAndNoOrientation( "nav2_navfn_planner/NavfnPlanner", 0.0 );
}
198 TEST( testPluginMap, NavFnVerySmallPath )
{
testSmallPathValidityAndOrientation( "nav2_navfn_planner/NavfnPlanner", 0.00001 );
}
203 TEST( testPluginMap, NavFnVerySmallPathN )
{
testSmallPathValidityAndNoOrientation( "nav2_navfn_planner/NavfnPlanner", 0.00001 );
}
208 TEST( testPluginMap, NavFnBelowCostmapResolution )
{
testSmallPathValidityAndOrientation( "nav2_navfn_planner/NavfnPlanner", 0.09 );
}
213 TEST( testPluginMap, NavFnBelowCostmapResolutionN )
{
testSmallPathValidityAndNoOrientation( "nav2_navfn_planner/NavfnPlanner", 0.09 );
}
218 TEST( testPluginMap, NavFnJustAboveCostmapResolution )
{
testSmallPathValidityAndOrientation( "nav2_navfn_planner/NavfnPlanner", 0.102 );
}
223 TEST( testPluginMap, NavFnJustAboveCostmapResolutionN )
{
testSmallPathValidityAndNoOrientation( "nav2_navfn_planner/NavfnPlanner", 0.102 );
}
228 TEST( testPluginMap, NavFnAboveCostmapResolution )
{
testSmallPathValidityAndOrientation( "nav2_navfn_planner/NavfnPlanner", 1.5 );
}
233 TEST( testPluginMap, NavFnAboveCostmapResolutionN )
{
testSmallPathValidityAndNoOrientation( "nav2_navfn_planner/NavfnPlanner", 1.5 );
}
238 TEST( testPluginMap, ThetaStarEqualStartGoal )
{
testSmallPathValidityAndOrientation( "nav2_theta_star_planner/ThetaStarPlanner", 0.0 );
}
243 TEST( testPluginMap, ThetaStarEqualStartGoalN )
{
testSmallPathValidityAndNoOrientation( "nav2_theta_star_planner/ThetaStarPlanner", 0.0 );
}
248 TEST( testPluginMap, ThetaStarVerySmallPath )
{
testSmallPathValidityAndOrientation( "nav2_theta_star_planner/ThetaStarPlanner", 0.00001 );
}
253 TEST( testPluginMap, ThetaStarVerySmallPathN )
{
testSmallPathValidityAndNoOrientation( "nav2_theta_star_planner/ThetaStarPlanner", 0.00001 );
}
258 TEST( testPluginMap, ThetaStarBelowCostmapResolution )
{
testSmallPathValidityAndOrientation( "nav2_theta_star_planner/ThetaStarPlanner", 0.09 );
}
263 TEST( testPluginMap, ThetaStarBelowCostmapResolutionN )
{
testSmallPathValidityAndNoOrientation( "nav2_theta_star_planner/ThetaStarPlanner", 0.09 );
}
268 TEST( testPluginMap, ThetaStarJustAboveCostmapResolution )
{
testSmallPathValidityAndOrientation( "nav2_theta_star_planner/ThetaStarPlanner", 0.102 );
}
273 TEST( testPluginMap, ThetaStarJustAboveCostmapResolutionN )
{
testSmallPathValidityAndNoOrientation( "nav2_theta_star_planner/ThetaStarPlanner", 0.102 );
}
278 TEST( testPluginMap, ThetaStarAboveCostmapResolution )
{
testSmallPathValidityAndOrientation( "nav2_theta_star_planner/ThetaStarPlanner", 1.5 );
}
283 TEST( testPluginMap, ThetaStarAboveCostmapResolutionN )
{
testSmallPathValidityAndNoOrientation( "nav2_theta_star_planner/ThetaStarPlanner", 1.5 );
}
288 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <gtest/gtest.h>
#include <memory>
#include <vector>
#include <iostream>
#include "rclcpp/rclcpp.hpp"
#include "planner_tester.hpp"
using namespace std::chrono_literals;
using nav2_system_tests::PlannerTester;
using nav2_util::TestCostmap;
using ComputePathToPoseCommand = geometry_msgs::msg::PoseStamped;
using ComputePathToPoseResult = nav_msgs::msg::Path;
31 TEST( testWithHundredRandomEndPoints, testWithHundredRandomEndPoints )
{
auto obj = std::make_shared<PlannerTester>( );
obj->activate( );
obj->loadDefaultMap( );
bool success = false;
int num_tries = 3;
for ( int i = 0; i != num_tries; i++ ) {
success = success || obj->defaultPlannerRandomTests( 100, 0.1 );
if ( success ) {
break;
}
}
EXPECT_EQ( true, success );
}
49 int main( int argc, char ** argv )
{
::testing::InitGoogleTest( &argc, argv );
// initialize ROS
rclcpp::init( argc, argv );
bool all_successful = RUN_ALL_TESTS( );
// shutdown ROS
rclcpp::shutdown( );
return all_successful;
}
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <random>
#include <string>
#include <vector>
#include <memory>
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_lifecycle_manager/lifecycle_manager_client.hpp"
#include "rcutils/cmdline_parser.h"
using namespace std::chrono_literals;
26 int main( int argc, char ** argv )
{
rclcpp::init( argc, argv );
RCLCPP_INFO( rclcpp::get_logger( "test_updown" ), "Initializing test" );
auto node = std::make_shared<rclcpp::Node>( "lifecycle_manager_service_client" );
nav2_lifecycle_manager::LifecycleManagerClient client_nav( "lifecycle_manager_navigation", node );
nav2_lifecycle_manager::LifecycleManagerClient client_loc( "lifecycle_manager_localization", node );
bool test_passed = true;
// Wait for a few seconds to let all of the nodes come up
std::this_thread::sleep_for( 5s );
// Start the nav2 system, bringing it to the ACTIVE state
client_nav.startup( );
client_loc.startup( );
// Wait for a couple secs to make sure the nodes have processed all discovery
// info before starting
RCLCPP_INFO( rclcpp::get_logger( "test_updown" ), "Waiting for nodes to be active" );
std::this_thread::sleep_for( 2s );
// The system should now be active
int retries = 0;
while ( ( client_nav.is_active( ) != nav2_lifecycle_manager::SystemStatus::ACTIVE ) &&
( client_loc.is_active( ) != nav2_lifecycle_manager::SystemStatus::ACTIVE ) &&
( retries < 10 ) )
{
std::this_thread::sleep_for( 2s );
retries++;
}
if ( retries == 10 ) {
// the system isn't active
RCLCPP_ERROR( rclcpp::get_logger( "test_updown" ), "System startup failed" );
test_passed = false;
}
// Shut down the nav2 system, bringing it to the FINALIZED state
client_nav.shutdown( );
client_loc.shutdown( );
if ( test_passed ) {
RCLCPP_INFO(
rclcpp::get_logger( "test_updown" ),
"**************************************************** TEST PASSED!" );
} else {
RCLCPP_INFO(
rclcpp::get_logger( "test_updown" ),
"**************************************************** TEST FAILED!" );
}
rclcpp::shutdown( );
return 0;
}
// Copyright 2020 Anshumaan Singh
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_THETA_STAR_PLANNER__THETA_STAR_HPP_
#define NAV2_THETA_STAR_PLANNER__THETA_STAR_HPP_
#include <cmath>
#include <chrono>
#include <vector>
#include <queue>
#include <algorithm>
#include "rclcpp/rclcpp.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
const double INF_COST = DBL_MAX;
const int LETHAL_COST = 252;
struct coordsM
{
int x, y;
};
struct coordsW
{
double x, y;
};
struct tree_node
{
int x, y;
double g = INF_COST;
double h = INF_COST;
const tree_node * parent_id = nullptr;
bool is_in_queue = false;
double f = INF_COST;
};
struct comp
{
bool operator( )( const tree_node * p1, const tree_node * p2 )
{
return ( p1->f ) > ( p2->f );
}
};
namespace theta_star
{
59 class ThetaStar
{
public:
coordsM src_{}, dst_{};
nav2_costmap_2d::Costmap2D * costmap_{};
/// weight on the costmap traversal cost
double w_traversal_cost_;
/// weight on the euclidean distance cost ( used for calculations of g_cost )
double w_euc_cost_;
/// weight on the heuristic cost ( used for h_cost calculations )
double w_heuristic_cost_;
/// parameter to set the number of adjacent nodes to be searched on
int how_many_corners_;
/// the x-directional and y-directional lengths of the map respectively
int size_x_, size_y_;
76 ThetaStar( );
78 ~ThetaStar( ) = default;
/**
* @brief it iteratively searches upon the nodes in the queue ( open list ) until the
* current node is the goal pose or the size of queue becomes 0
* @param raw_path is used to return the path obtained by executing the algorithm
* @return true if a path is found, false if no path is found in between the start and goal pose
*/
86 bool generatePath( std::vector<coordsW> & raw_path );
/**
* @brief this function checks whether the cost of a point( cx, cy ) on the costmap is less than the LETHAL_COST
* @return the result of the check
*/
92 inline bool isSafe( const int & cx, const int & cy ) const
{
return costmap_->getCost( cx, cy ) < LETHAL_COST;
}
/**
* @brief initialises the values of the start and goal points
*/
100 void setStartAndGoal(
const geometry_msgs::msg::PoseStamped & start,
const geometry_msgs::msg::PoseStamped & goal );
/**
* @brief checks whether the start and goal points have costmap costs greater than LETHAL_COST
* @return true if the cost of any one of the points is greater than LETHAL_COST
*/
108 bool isUnsafeToPlan( ) const
{
return !( isSafe( src_.x, src_.y ) ) || !( isSafe( dst_.x, dst_.y ) );
}
int nodes_opened = 0;
protected:
/// for the coordinates ( x, y ), it stores at node_position_[size_x_ * y + x],
/// the pointer to the location at which the data of the node is present in nodes_data_
/// it is initialised with size_x_ * size_y_ elements
/// and its number of elements increases to account for a change in map size
std::vector<tree_node *> node_position_;
/// the vector nodes_data_ stores the coordinates, costs and index of the parent node,
/// and whether or not the node is present in queue_, for all the nodes searched
/// it is initialised with no elements
/// and its size increases depending on the number of nodes searched
std::vector<tree_node> nodes_data_;
/// this is the priority queue ( open_list ) to select the next node to be expanded
std::priority_queue<tree_node *, std::vector<tree_node *>, comp> queue_;
/// it is a counter like variable used to generate consecutive indices
/// such that the data for all the nodes ( in open and closed lists ) could be stored
/// consecutively in nodes_data_
int index_generated_;
const coordsM moves[8] = {{0, 1},
{0, -1},
{1, 0},
{-1, 0},
{1, -1},
{-1, 1},
{1, 1},
{-1, -1}};
tree_node * exp_node;
/** @brief it performs a line of sight ( los ) check between the current node and the parent node of its parent node;
* if an los is found and the new costs calculated are lesser, then the cost and parent node
* of the current node is updated
* @param data of the current node
*/
void resetParent( tree_node * curr_data );
/**
* @brief this function expands the current node
* @param curr_data used to send the data of the current node
* @param curr_id used to send the index of the current node as stored in nodes_position_
*/
void setNeighbors( const tree_node * curr_data );
/**
* @brief performs the line of sight check using Bresenham's Algorithm,
* and has been modified to calculate the traversal cost incurred in a straight line path between
* the two points whose coordinates are ( x0, y0 ) and ( x1, y1 )
* @param sl_cost is used to return the cost thus incurred
* @return true if a line of sight exists between the points
*/
bool losCheck(
const int & x0, const int & y0, const int & x1, const int & y1,
double & sl_cost ) const;
/**
* @brief it returns the path by backtracking from the goal to the start, by using their parent nodes
* @param raw_points used to return the path thus found
* @param curr_id sends in the index of the goal coordinate, as stored in nodes_position
*/
void backtrace( std::vector<coordsW> & raw_points, const tree_node * curr_n ) const;
/**
* @brief it is an overloaded function to ease the cost calculations while performing the LOS check
* @param cost denotes the total straight line traversal cost; it adds the traversal cost for the node ( cx, cy ) at every instance; it is also being returned
* @return false if the traversal cost is greater than / equal to the LETHAL_COST and true otherwise
*/
bool isSafe( const int & cx, const int & cy, double & cost ) const
{
double curr_cost = getCost( cx, cy );
if ( curr_cost < LETHAL_COST ) {
cost += w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST;
return true;
} else {
return false;
}
}
/*
* @brief this function scales the costmap cost by shifting the origin to 25 and then multiply
* the actual costmap cost by 0.9 to keep the output in the range of [25, 255 )
*/
inline double getCost( const int & cx, const int & cy ) const
{
return 26 + 0.9 * costmap_->getCost( cx, cy );
}
/**
* @brief for the point( cx, cy ), its traversal cost is calculated by
* <parameter>*( <actual_traversal_cost_from_costmap> )^2/( <max_cost> )^2
* @return the traversal cost thus calculated
*/
inline double getTraversalCost( const int & cx, const int & cy )
{
double curr_cost = getCost( cx, cy );
return w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST;
}
/**
* @brief calculates the piecewise straight line euclidean distances by
* <euc_cost_parameter>*<euclidean distance between the points ( ax, ay ) and ( bx, by )>
* @return the distance thus calculated
*/
inline double getEuclideanCost( const int & ax, const int & ay, const int & bx, const int & by )
{
return w_euc_cost_ * std::hypot( ax - bx, ay - by );
}
/**
* @brief for the point( cx, cy ), its heuristic cost is calculated by
* <heuristic_cost_parameter>*<euclidean distance between the point and goal>
* @return the heuristic cost
*/
inline double getHCost( const int & cx, const int & cy )
{
return w_heuristic_cost_ * std::hypot( cx - dst_.x, cy - dst_.y );
}
/**
* @brief checks if the given coordinates( cx, cy ) lies within the map
* @return the result of the check
*/
inline bool withinLimits( const int & cx, const int & cy ) const
{
return cx >= 0 && cx < size_x_ && cy >= 0 && cy < size_y_;
}
/**
* @brief checks if the coordinates of a node is the goal or not
* @return the result of the check
*/
inline bool isGoal( const tree_node & this_node ) const
{
return this_node.x == dst_.x && this_node.y == dst_.y;
}
/**
* @brief initialises the node_position_ vector by storing -1 as index for all points( x, y ) within the limits of the map
* @param size_inc is used to increase the number of elements in node_position_ in case the size of the map increases
*/
void initializePosn( int size_inc = 0 );
/**
* @brief it stores id_this in node_position_ at the index [ size_x_*cy + cx ]
* @param id_this a pointer to the location at which the data of the point( cx, cy ) is stored in nodes_data_
*/
inline void addIndex( const int & cx, const int & cy, tree_node * node_this )
{
node_position_[size_x_ * cy + cx] = node_this;
}
/**
* @brief retrieves the pointer of the location at which the data of the point( cx, cy ) is stored in nodes_data
* @return id_this is the pointer to that location
*/
inline tree_node * getIndex( const int & cx, const int & cy )
{
return node_position_[size_x_ * cy + cx];
}
/**
* @brief this function depending on the size of the nodes_data_ vector allots space to store the data for a node( x, y )
* @param id_this is the index at which the data is stored/has to be stored for that node
*/
void addToNodesData( const int & id_this )
{
if ( static_cast<int>( nodes_data_.size( ) ) <= id_this ) {
nodes_data_.push_back( {} );
} else {
nodes_data_[id_this] = {};
}
}
/**
* @brief initialises the values of global variables at beginning of the execution of the generatePath function
*/
void resetContainers( );
/**
* @brief clears the priority queue after each execution of the generatePath function
*/
void clearQueue( )
{
queue_ = std::priority_queue<tree_node *, std::vector<tree_node *>, comp>( );
}
};
} // namespace theta_star
#endif // NAV2_THETA_STAR_PLANNER__THETA_STAR_HPP_
// Copyright 2020 Anshumaan Singh
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_THETA_STAR_PLANNER__THETA_STAR_PLANNER_HPP_
#define NAV2_THETA_STAR_PLANNER__THETA_STAR_PLANNER_HPP_
#include <iostream>
#include <cmath>
#include <string>
#include <chrono>
#include <queue>
#include <algorithm>
#include <memory>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "nav2_core/global_planner.hpp"
#include "nav_msgs/msg/path.hpp"
#include "nav2_util/robot_utils.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav2_costmap_2d/cost_values.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_theta_star_planner/theta_star.hpp"
#include "nav2_util/geometry_utils.hpp"
using rcl_interfaces::msg::ParameterType;
namespace nav2_theta_star_planner
{
42 class ThetaStarPlanner : public nav2_core::GlobalPlanner
{
public:
void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros ) override;
void cleanup( ) override;
void activate( ) override;
void deactivate( ) override;
nav_msgs::msg::Path createPlan(
const geometry_msgs::msg::PoseStamped & start,
const geometry_msgs::msg::PoseStamped & goal ) override;
protected:
std::shared_ptr<tf2_ros::Buffer> tf_;
rclcpp::Clock::SharedPtr clock_;
rclcpp::Logger logger_{rclcpp::get_logger( "ThetaStarPlanner" )};
std::string global_frame_, name_;
bool use_final_approach_orientation_;
// parent node weak ptr
rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node_;
std::unique_ptr<theta_star::ThetaStar> planner_;
// Dynamic parameters handler
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
/**
* @brief the function responsible for calling the algorithm and retrieving a path from it
* @return global_path is the planned path to be taken
*/
80 void getPlan( nav_msgs::msg::Path & global_path );
/**
* @brief interpolates points between the consecutive waypoints of the path
* @param raw_path is used to send in the path received from the planner
* @param dist_bw_points is used to send in the interpolation_resolution ( which has been set as the costmap resolution )
* @return the final path with waypoints at a distance of the value of interpolation_resolution of each other
*/
88 static nav_msgs::msg::Path linearInterpolation(
const std::vector<coordsW> & raw_path,
const double & dist_bw_points );
/**
* @brief Callback executed when a paramter change is detected
* @param parameters list of changed parameters
*/
rcl_interfaces::msg::SetParametersResult
97 dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters );
};
} // namespace nav2_theta_star_planner
#endif // NAV2_THETA_STAR_PLANNER__THETA_STAR_PLANNER_HPP_
1 // Copyright 2020 Anshumaan Singh
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <vector>
#include "nav2_theta_star_planner/theta_star.hpp"
namespace theta_star
{
21 ThetaStar::ThetaStar( )
: w_traversal_cost_( 1.0 ),
w_euc_cost_( 2.0 ),
w_heuristic_cost_( 1.0 ),
how_many_corners_( 8 ),
size_x_( 0 ),
size_y_( 0 ),
index_generated_( 0 )
{
exp_node = new tree_node;
}
33 void ThetaStar::setStartAndGoal(
34 const geometry_msgs::msg::PoseStamped & start,
35 const geometry_msgs::msg::PoseStamped & goal )
{
unsigned int s[2], d[2];
costmap_->worldToMap( start.pose.position.x, start.pose.position.y, s[0], s[1] );
costmap_->worldToMap( goal.pose.position.x, goal.pose.position.y, d[0], d[1] );
src_ = {static_cast<int>( s[0] ), static_cast<int>( s[1] )};
dst_ = {static_cast<int>( d[0] ), static_cast<int>( d[1] )};
}
45 bool ThetaStar::generatePath( std::vector<coordsW> & raw_path )
{
resetContainers( );
addToNodesData( index_generated_ );
double src_g_cost = getTraversalCost( src_.x, src_.y ), src_h_cost = getHCost( src_.x, src_.y );
nodes_data_[index_generated_] =
{src_.x, src_.y, src_g_cost, src_h_cost, &nodes_data_[index_generated_], true,
src_g_cost + src_h_cost};
queue_.push( {&nodes_data_[index_generated_]} );
addIndex( src_.x, src_.y, &nodes_data_[index_generated_] );
tree_node * curr_data = &nodes_data_[index_generated_];
index_generated_++;
nodes_opened = 0;
while ( !queue_.empty( ) ) {
nodes_opened++;
if ( isGoal( *curr_data ) ) {
break;
}
resetParent( curr_data );
setNeighbors( curr_data );
curr_data = queue_.top( );
queue_.pop( );
}
if ( queue_.empty( ) ) {
raw_path.clear( );
return false;
}
backtrace( raw_path, curr_data );
clearQueue( );
return true;
}
84 void ThetaStar::resetParent( tree_node * curr_data )
{
double g_cost, los_cost = 0;
curr_data->is_in_queue = false;
const tree_node * curr_par = curr_data->parent_id;
const tree_node * maybe_par = curr_par->parent_id;
if ( losCheck( curr_data->x, curr_data->y, maybe_par->x, maybe_par->y, los_cost ) ) {
g_cost = maybe_par->g +
getEuclideanCost( curr_data->x, curr_data->y, maybe_par->x, maybe_par->y ) + los_cost;
if ( g_cost < curr_data->g ) {
curr_data->parent_id = maybe_par;
curr_data->g = g_cost;
curr_data->f = g_cost + curr_data->h;
}
}
}
103 void ThetaStar::setNeighbors( const tree_node * curr_data )
{
int mx, my;
tree_node * m_id = nullptr;
double g_cost, h_cost, cal_cost;
for ( int i = 0; i < how_many_corners_; i++ ) {
mx = curr_data->x + moves[i].x;
my = curr_data->y + moves[i].y;
if ( withinLimits( mx, my ) ) {
if ( !isSafe( mx, my ) ) {
continue;
}
} else {
continue;
}
g_cost = curr_data->g + getEuclideanCost( curr_data->x, curr_data->y, mx, my ) +
getTraversalCost( mx, my );
m_id = getIndex( mx, my );
if ( m_id == nullptr ) {
addToNodesData( index_generated_ );
m_id = &nodes_data_[index_generated_];
addIndex( mx, my, m_id );
index_generated_++;
}
exp_node = m_id;
h_cost = getHCost( mx, my );
cal_cost = g_cost + h_cost;
if ( exp_node->f > cal_cost ) {
exp_node->g = g_cost;
exp_node->h = h_cost;
exp_node->f = cal_cost;
exp_node->parent_id = curr_data;
if ( !exp_node->is_in_queue ) {
exp_node->x = mx;
exp_node->y = my;
exp_node->is_in_queue = true;
queue_.push( {m_id} );
}
}
}
}
152 void ThetaStar::backtrace( std::vector<coordsW> & raw_points, const tree_node * curr_n ) const
{
std::vector<coordsW> path_rev;
coordsW world{};
do {
costmap_->mapToWorld( curr_n->x, curr_n->y, world.x, world.y );
path_rev.push_back( world );
if ( path_rev.size( ) > 1 ) {
curr_n = curr_n->parent_id;
}
} while ( curr_n->parent_id != curr_n );
costmap_->mapToWorld( curr_n->x, curr_n->y, world.x, world.y );
path_rev.push_back( world );
raw_points.reserve( path_rev.size( ) );
for ( int i = static_cast<int>( path_rev.size( ) ) - 1; i >= 0; i-- ) {
raw_points.push_back( path_rev[i] );
}
}
172 bool ThetaStar::losCheck(
const int & x0, const int & y0, const int & x1, const int & y1,
double & sl_cost ) const
{
sl_cost = 0;
int cx, cy;
int dy = abs( y1 - y0 ), dx = abs( x1 - x0 ), f = 0;
int sx, sy;
sx = x1 > x0 ? 1 : -1;
sy = y1 > y0 ? 1 : -1;
int u_x = ( sx - 1 ) / 2;
int u_y = ( sy - 1 ) / 2;
cx = x0;
cy = y0;
if ( dx >= dy ) {
while ( cx != x1 ) {
f += dy;
if ( f >= dx ) {
if ( !isSafe( cx + u_x, cy + u_y, sl_cost ) ) {
return false;
}
cy += sy;
f -= dx;
}
if ( f != 0 && !isSafe( cx + u_x, cy + u_y, sl_cost ) ) {
return false;
}
if ( dy == 0 && !isSafe( cx + u_x, cy, sl_cost ) && !isSafe( cx + u_x, cy - 1, sl_cost ) ) {
return false;
}
cx += sx;
}
} else {
while ( cy != y1 ) {
f = f + dx;
if ( f >= dy ) {
if ( !isSafe( cx + u_x, cy + u_y, sl_cost ) ) {
return false;
}
cx += sx;
f -= dy;
}
if ( f != 0 && !isSafe( cx + u_x, cy + u_y, sl_cost ) ) {
return false;
}
if ( dx == 0 && !isSafe( cx, cy + u_y, sl_cost ) && !isSafe( cx - 1, cy + u_y, sl_cost ) ) {
return false;
}
cy += sy;
}
}
return true;
}
229 void ThetaStar::resetContainers( )
{
index_generated_ = 0;
int last_size_x = size_x_;
int last_size_y = size_y_;
int curr_size_x = static_cast<int>( costmap_->getSizeInCellsX( ) );
int curr_size_y = static_cast<int>( costmap_->getSizeInCellsY( ) );
if ( ( ( last_size_x != curr_size_x ) || ( last_size_y != curr_size_y ) ) &&
static_cast<int>( node_position_.size( ) ) < ( curr_size_x * curr_size_y ) )
{
initializePosn( curr_size_y * curr_size_x - last_size_y * last_size_x );
nodes_data_.reserve( curr_size_x * curr_size_y );
} else {
initializePosn( );
}
size_x_ = curr_size_x;
size_y_ = curr_size_y;
}
248 void ThetaStar::initializePosn( int size_inc )
{
int i = 0;
if ( !node_position_.empty( ) ) {
for ( ; i < size_x_ * size_y_; i++ ) {
node_position_[i] = nullptr;
}
}
for ( ; i < size_inc; i++ ) {
node_position_.push_back( nullptr );
}
}
} // namespace theta_star
1 // Copyright 2020 Anshumaan Singh
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <vector>
#include <memory>
#include <string>
#include "nav2_theta_star_planner/theta_star_planner.hpp"
#include "nav2_theta_star_planner/theta_star.hpp"
namespace nav2_theta_star_planner
{
23 void ThetaStarPlanner::configure(
24 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
25 std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
26 std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros )
{
planner_ = std::make_unique<theta_star::ThetaStar>( );
parent_node_ = parent;
auto node = parent_node_.lock( );
logger_ = node->get_logger( );
clock_ = node->get_clock( );
name_ = name;
tf_ = tf;
planner_->costmap_ = costmap_ros->getCostmap( );
global_frame_ = costmap_ros->getGlobalFrameID( );
nav2_util::declare_parameter_if_not_declared(
node, name_ + ".how_many_corners", rclcpp::ParameterValue( 8 ) );
node->get_parameter( name_ + ".how_many_corners", planner_->how_many_corners_ );
if ( planner_->how_many_corners_ != 8 && planner_->how_many_corners_ != 4 ) {
planner_->how_many_corners_ = 8;
RCLCPP_WARN( logger_, "Your value for - .how_many_corners was overridden, and is now set to 8" );
}
nav2_util::declare_parameter_if_not_declared(
node, name_ + ".w_euc_cost", rclcpp::ParameterValue( 1.0 ) );
node->get_parameter( name_ + ".w_euc_cost", planner_->w_euc_cost_ );
nav2_util::declare_parameter_if_not_declared(
node, name_ + ".w_traversal_cost", rclcpp::ParameterValue( 2.0 ) );
node->get_parameter( name_ + ".w_traversal_cost", planner_->w_traversal_cost_ );
planner_->w_heuristic_cost_ = planner_->w_euc_cost_ < 1.0 ? planner_->w_euc_cost_ : 1.0;
nav2_util::declare_parameter_if_not_declared(
node, name + ".use_final_approach_orientation", rclcpp::ParameterValue( false ) );
node->get_parameter( name + ".use_final_approach_orientation", use_final_approach_orientation_ );
}
63 void ThetaStarPlanner::cleanup( )
{
RCLCPP_INFO( logger_, "CleaningUp plugin %s of type nav2_theta_star_planner", name_.c_str( ) );
planner_.reset( );
}
69 void ThetaStarPlanner::activate( )
{
RCLCPP_INFO( logger_, "Activating plugin %s of type nav2_theta_star_planner", name_.c_str( ) );
// Add callback for dynamic parameters
auto node = parent_node_.lock( );
dyn_params_handler_ = node->add_on_set_parameters_callback(
std::bind( &ThetaStarPlanner::dynamicParametersCallback, this, std::placeholders::_1 ) );
}
78 void ThetaStarPlanner::deactivate( )
{
RCLCPP_INFO( logger_, "Deactivating plugin %s of type nav2_theta_star_planner", name_.c_str( ) );
}
83 nav_msgs::msg::Path ThetaStarPlanner::createPlan(
84 const geometry_msgs::msg::PoseStamped & start,
85 const geometry_msgs::msg::PoseStamped & goal )
{
nav_msgs::msg::Path global_path;
auto start_time = std::chrono::steady_clock::now( );
// Corner case of start and goal beeing on the same cell
unsigned int mx_start, my_start, mx_goal, my_goal;
planner_->costmap_->worldToMap( start.pose.position.x, start.pose.position.y, mx_start, my_start );
planner_->costmap_->worldToMap( goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal );
if ( mx_start == mx_goal && my_start == my_goal ) {
if ( planner_->costmap_->getCost( mx_start, my_start ) == nav2_costmap_2d::LETHAL_OBSTACLE ) {
RCLCPP_WARN( logger_, "Failed to create a unique pose path because of obstacles" );
return global_path;
}
global_path.header.stamp = clock_->now( );
global_path.header.frame_id = global_frame_;
geometry_msgs::msg::PoseStamped pose;
pose.header = global_path.header;
pose.pose.position.z = 0.0;
pose.pose = start.pose;
// if we have a different start and goal orientation, set the unique path pose to the goal
// orientation, unless use_final_approach_orientation=true where we need it to be the start
// orientation to avoid movement from the local planner
if ( start.pose.orientation != goal.pose.orientation && !use_final_approach_orientation_ ) {
pose.pose.orientation = goal.pose.orientation;
}
global_path.poses.push_back( pose );
return global_path;
}
planner_->setStartAndGoal( start, goal );
RCLCPP_DEBUG(
logger_, "Got the src and dst... ( %i, %i ) && ( %i, %i )",
planner_->src_.x, planner_->src_.y, planner_->dst_.x, planner_->dst_.y );
getPlan( global_path );
// check if a plan is generated
size_t plan_size = global_path.poses.size( );
if ( plan_size > 0 ) {
global_path.poses.back( ).pose.orientation = goal.pose.orientation;
}
// If use_final_approach_orientation=true, interpolate the last pose orientation from the
// previous pose to set the orientation to the 'final approach' orientation of the robot so
// it does not rotate.
// And deal with corner case of plan of length 1
if ( use_final_approach_orientation_ ) {
if ( plan_size == 1 ) {
global_path.poses.back( ).pose.orientation = start.pose.orientation;
} else if ( plan_size > 1 ) {
double dx, dy, theta;
auto last_pose = global_path.poses.back( ).pose.position;
auto approach_pose = global_path.poses[plan_size - 2].pose.position;
dx = last_pose.x - approach_pose.x;
dy = last_pose.y - approach_pose.y;
theta = atan2( dy, dx );
global_path.poses.back( ).pose.orientation =
nav2_util::geometry_utils::orientationAroundZAxis( theta );
}
}
auto stop_time = std::chrono::steady_clock::now( );
auto dur = std::chrono::duration_cast<std::chrono::microseconds>( stop_time - start_time );
RCLCPP_DEBUG( logger_, "the time taken is : %i", static_cast<int>( dur.count( ) ) );
RCLCPP_DEBUG( logger_, "the nodes_opened are: %i", planner_->nodes_opened );
return global_path;
}
153 void ThetaStarPlanner::getPlan( nav_msgs::msg::Path & global_path )
{
std::vector<coordsW> path;
if ( planner_->isUnsafeToPlan( ) ) {
RCLCPP_ERROR( logger_, "Either of the start or goal pose are an obstacle! " );
global_path.poses.clear( );
} else if ( planner_->generatePath( path ) ) {
global_path = linearInterpolation( path, planner_->costmap_->getResolution( ) );
} else {
RCLCPP_ERROR( logger_, "Could not generate path between the given poses" );
global_path.poses.clear( );
}
global_path.header.stamp = clock_->now( );
global_path.header.frame_id = global_frame_;
}
169 nav_msgs::msg::Path ThetaStarPlanner::linearInterpolation(
170 const std::vector<coordsW> & raw_path,
const double & dist_bw_points )
{
nav_msgs::msg::Path pa;
geometry_msgs::msg::PoseStamped p1;
for ( unsigned int j = 0; j < raw_path.size( ) - 1; j++ ) {
coordsW pt1 = raw_path[j];
p1.pose.position.x = pt1.x;
p1.pose.position.y = pt1.y;
pa.poses.push_back( p1 );
coordsW pt2 = raw_path[j + 1];
double distance = std::hypot( pt2.x - pt1.x, pt2.y - pt1.y );
int loops = static_cast<int>( distance / dist_bw_points );
double sin_alpha = ( pt2.y - pt1.y ) / distance;
double cos_alpha = ( pt2.x - pt1.x ) / distance;
for ( int k = 1; k < loops; k++ ) {
p1.pose.position.x = pt1.x + k * dist_bw_points * cos_alpha;
p1.pose.position.y = pt1.y + k * dist_bw_points * sin_alpha;
pa.poses.push_back( p1 );
}
}
return pa;
}
rcl_interfaces::msg::SetParametersResult
198 ThetaStarPlanner::dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters )
{
rcl_interfaces::msg::SetParametersResult result;
for ( auto parameter : parameters ) {
const auto & type = parameter.get_type( );
const auto & name = parameter.get_name( );
if ( type == ParameterType::PARAMETER_INTEGER ) {
if ( name == name_ + ".how_many_corners" ) {
planner_->how_many_corners_ = parameter.as_int( );
}
} else if ( type == ParameterType::PARAMETER_DOUBLE ) {
if ( name == name_ + ".w_euc_cost" ) {
planner_->w_euc_cost_ = parameter.as_double( );
} else if ( name == name_ + ".w_traversal_cost" ) {
planner_->w_traversal_cost_ = parameter.as_double( );
}
} else if ( type == ParameterType::PARAMETER_BOOL ) {
if ( name == name_ + ".use_final_approach_orientation" ) {
use_final_approach_orientation_ = parameter.as_bool( );
}
}
}
result.successful = true;
return result;
}
} // namespace nav2_theta_star_planner
#include "pluginlib/class_list_macros.hpp"
229 PLUGINLIB_EXPORT_CLASS( nav2_theta_star_planner::ThetaStarPlanner, nav2_core::GlobalPlanner )
1 // Copyright 2020 Anshumaan Singh
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "nav2_theta_star_planner/theta_star.hpp"
#include "nav2_theta_star_planner/theta_star_planner.hpp"
22 class init_rclcpp
{
public:
25 init_rclcpp( ) {rclcpp::init( 0, nullptr );}
26 ~init_rclcpp( ) {rclcpp::shutdown( );}
};
/// class created to access the protected members of the ThetaStar class
/// u is used as shorthand for use
31 class test_theta_star : public theta_star::ThetaStar
{
public:
34 int getSizeOfNodePosition( )
{
return static_cast<int>( node_position_.size( ) );
}
39 bool ulosCheck( const int & x0, const int & y0, const int & x1, const int & y1, double & sl_cost )
{
return losCheck( x0, y0, x1, y1, sl_cost );
}
44 bool uwithinLimits( const int & cx, const int & cy ) {return withinLimits( cx, cy );}
46 bool uisGoal( const tree_node & this_node ) {return isGoal( this_node );}
48 void uinitializePosn( int size_inc = 0 )
{
node_position_.reserve( size_x_ * size_y_ ); initializePosn( size_inc );
}
53 void uaddIndex( const int & cx, const int & cy ) {addIndex( cx, cy, &nodes_data_[0] );}
55 tree_node * ugetIndex( const int & cx, const int & cy ) {return getIndex( cx, cy );}
57 tree_node * test_getIndex( ) {return &nodes_data_[0];}
59 void uaddToNodesData( const int & id ) {addToNodesData( id );}
61 void uresetContainers( ) {nodes_data_.clear( ); resetContainers( );}
63 bool runAlgo( std::vector<coordsW> & path )
{
if ( !isUnsafeToPlan( ) ) {
return generatePath( path );
}
return false;
}
};
init_rclcpp node;
// Tests meant to test the algorithm itself and its helper functions
75 TEST( ThetaStarTest, test_theta_star ) {
auto planner_ = std::make_unique<test_theta_star>( );
planner_->costmap_ = new nav2_costmap_2d::Costmap2D( 50, 50, 1.0, 0.0, 0.0, 0 );
for ( int i = 7; i <= 14; i++ ) {
for ( int j = 7; j <= 14; j++ ) {
planner_->costmap_->setCost( i, j, 253 );
}
}
coordsM s = {5, 5}, g = {18, 18};
int size_x = 20, size_y = 20;
planner_->size_x_ = size_x;
planner_->size_y_ = size_y;
geometry_msgs::msg::PoseStamped start, goal;
start.pose.position.x = s.x;
start.pose.position.y = s.y;
start.pose.orientation.w = 1.0;
goal.pose.position.x = g.x;
goal.pose.position.y = g.y;
goal.pose.orientation.w = 1.0;
/// Check if the setStartAndGoal function works properly
planner_->setStartAndGoal( start, goal );
EXPECT_TRUE( planner_->src_.x == s.x && planner_->src_.y == s.y );
EXPECT_TRUE( planner_->dst_.x == g.x && planner_->dst_.y == g.y );
/// Check if the initializePosn function works properly
planner_->uinitializePosn( size_x * size_y );
EXPECT_EQ( planner_->getSizeOfNodePosition( ), ( size_x * size_y ) );
/// Check if the withinLimits function works properly
EXPECT_TRUE( planner_->uwithinLimits( 18, 18 ) );
EXPECT_FALSE( planner_->uwithinLimits( 120, 140 ) );
tree_node n = {g.x, g.y, 120, 0, NULL, false, 20};
n.parent_id = &n;
/// Check if the isGoal function works properly
EXPECT_TRUE( planner_->uisGoal( n ) ); // both ( x, y ) are the goal coordinates
n.x = 25;
EXPECT_FALSE( planner_->uisGoal( n ) ); // only y coordinate matches with that of goal
n.x = g.x;
n.y = 20;
EXPECT_FALSE( planner_->uisGoal( n ) ); // only x coordinate matches with that of goal
n.x = 30;
EXPECT_FALSE( planner_->uisGoal( n ) ); // both ( x, y ) are different from the goal coordinate
/// Check if the isSafe functions work properly
EXPECT_TRUE( planner_->isSafe( 5, 5 ) ); // cost at this point is 0
EXPECT_FALSE( planner_->isSafe( 10, 10 ) ); // cost at this point is 253 ( >LETHAL_COST )
/// Check if the functions addIndex & getIndex work properly
coordsM c = {20, 30};
planner_->uaddToNodesData( 0 );
planner_->uaddIndex( c.x, c.y );
tree_node * c_node = planner_->ugetIndex( c.x, c.y );
EXPECT_EQ( c_node, planner_->test_getIndex( ) );
double sl_cost = 0.0;
/// Checking for the case where the losCheck should return the value as true
EXPECT_TRUE( planner_->ulosCheck( 2, 2, 7, 20, sl_cost ) );
/// and as false
EXPECT_FALSE( planner_->ulosCheck( 2, 2, 18, 18, sl_cost ) );
planner_->uresetContainers( );
std::vector<coordsW> path;
/// Check if the planner returns a path for the case where a path exists
EXPECT_TRUE( planner_->runAlgo( path ) );
EXPECT_GT( static_cast<int>( path.size( ) ), 0 );
/// and where it doesn't exist
path.clear( );
planner_->src_ = {10, 10};
EXPECT_FALSE( planner_->runAlgo( path ) );
EXPECT_EQ( static_cast<int>( path.size( ) ), 0 );
}
// Smoke tests meant to detect issues arising from the plugin part rather than the algorithm
149 TEST( ThetaStarPlanner, test_theta_star_planner ) {
rclcpp_lifecycle::LifecycleNode::SharedPtr life_node =
std::make_shared<rclcpp_lifecycle::LifecycleNode>( "ThetaStarPlannerTest" );
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros =
std::make_shared<nav2_costmap_2d::Costmap2DROS>( "global_costmap" );
costmap_ros->on_configure( rclcpp_lifecycle::State( ) );
geometry_msgs::msg::PoseStamped start, goal;
start.pose.position.x = 0.0;
start.pose.position.y = 0.0;
start.pose.orientation.w = 1.0;
goal = start;
auto planner_2d = std::make_unique<nav2_theta_star_planner::ThetaStarPlanner>( );
planner_2d->configure( life_node, "test", nullptr, costmap_ros );
planner_2d->activate( );
nav_msgs::msg::Path path = planner_2d->createPlan( start, goal );
EXPECT_GT( static_cast<int>( path.poses.size( ) ), 0 );
// test if the goal is unsafe
for ( int i = 7; i <= 14; i++ ) {
for ( int j = 7; j <= 14; j++ ) {
costmap_ros->getCostmap( )->setCost( i, j, 254 );
}
}
goal.pose.position.x = 1.0;
goal.pose.position.y = 1.0;
path = planner_2d->createPlan( start, goal );
EXPECT_EQ( static_cast<int>( path.poses.size( ) ), 0 );
planner_2d->deactivate( );
planner_2d->cleanup( );
planner_2d.reset( );
costmap_ros->on_cleanup( rclcpp_lifecycle::State( ) );
life_node.reset( );
costmap_ros.reset( );
}
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_UTIL__CLEAR_ENTIRELY_COSTMAP_SERVICE_CLIENT_HPP_
#define NAV2_UTIL__CLEAR_ENTIRELY_COSTMAP_SERVICE_CLIENT_HPP_
#include <string>
#include "nav2_util/service_client.hpp"
#include "std_srvs/srv/empty.hpp"
#include "nav2_msgs/srv/clear_entire_costmap.hpp"
namespace nav2_util
{
/**
* @class nav2_util::ClearEntirelyCostmapServiceClient
* @brief A service client to clear costmaps entirely
*/
29 class ClearEntirelyCostmapServiceClient
30 : public nav2_util::ServiceClient<nav2_msgs::srv::ClearEntireCostmap>
{
public:
/**
* @brief A constructor for nav2_util::ClearEntirelyCostmapServiceClient
*/
36 explicit ClearEntirelyCostmapServiceClient( const std::string & service_name )
: nav2_util::ServiceClient<nav2_msgs::srv::ClearEntireCostmap>( service_name )
{
}
using clearEntirelyCostmapServiceRequest =
nav2_util::ServiceClient<nav2_msgs::srv::ClearEntireCostmap>::RequestType;
using clearEntirelyCostmapServiceResponse =
nav2_util::ServiceClient<nav2_msgs::srv::ClearEntireCostmap>::ResponseType;
};
} // namespace nav2_util
#endif // NAV2_UTIL__CLEAR_ENTIRELY_COSTMAP_SERVICE_CLIENT_HPP_
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_UTIL__COSTMAP_HPP_
#define NAV2_UTIL__COSTMAP_HPP_
#include <vector>
#include <cstdint>
#include "rclcpp/rclcpp.hpp"
#include "nav2_msgs/msg/costmap.hpp"
#include "nav2_msgs/msg/costmap_meta_data.hpp"
#include "nav_msgs/msg/occupancy_grid.hpp"
namespace nav2_util
{
29 enum class TestCostmap
{
open_space,
bounded,
bottom_left_obstacle,
top_left_obstacle,
maze1,
maze2
};
/**
* @class nav2_util::Costmap
* @brief Class for a single layered costmap initialized from an
* occupancy grid representing the map.
*/
44 class Costmap
{
public:
typedef uint8_t CostValue;
/**
* @brief A constructor for nav2_util::Costmap
* @param node Ptr to a node
* @param trinary_costmap Whether the costmap should be trinary
* @param track_unknown_space Whether to track unknown space in costmap
* @param lethal_threshold The lethal space cost threshold to use
* @param unknown_cost_value Internal costmap cell value for unknown space
*/
57 Costmap(
58 rclcpp::Node * node, bool trinary_costmap = true, bool track_unknown_space = true,
int lethal_threshold = 100, int unknown_cost_value = -1 );
Costmap( ) = delete;
~Costmap( );
/**
* @brief Set the static map of this costmap
* @param occupancy_grid Occupancy grid to populate this costmap with
*/
void set_static_map( const nav_msgs::msg::OccupancyGrid & occupancy_grid );
/**
* @brief Set the test costmap type of this costmap
* @param testCostmapType Type of stored costmap to use
*/
void set_test_costmap( const TestCostmap & testCostmapType );
/**
* @brief Get a costmap message from this object
* @param specifications Parameters of costmap
* @return Costmap msg of this costmap
*/
nav2_msgs::msg::Costmap get_costmap( const nav2_msgs::msg::CostmapMetaData & specifications );
/**
* @brief Get a metadata message from this object
* @return Costmap metadata of this costmap
*/
nav2_msgs::msg::CostmapMetaData get_properties( ) {return costmap_properties_;}
/**
* @brief Get whether some coordinates are free
* @return bool if free
*/
bool is_free( const unsigned int x_coordinate, const unsigned int y_coordinate ) const;
/**
* @brief Get whether some index in the costmap is free
* @return bool if free
*/
98 bool is_free( const unsigned int index ) const;
// Mapping for often used cost values
static const CostValue no_information;
static const CostValue lethal_obstacle;
static const CostValue inscribed_inflated_obstacle;
static const CostValue medium_cost;
static const CostValue free_space;
private:
/**
* @brief Get data from the test
* @return data
*/
112 std::vector<uint8_t> get_test_data( const TestCostmap configuration );
/**
* @brief Get the interpreted value in the costmap
* @return uint value
*/
118 uint8_t interpret_value( const int8_t value ) const;
// Costmap isn't itself a node
rclcpp::Node * node_;
// TODO( orduno ): For now, only holding costs from static map
nav2_msgs::msg::CostmapMetaData costmap_properties_;
std::vector<uint8_t> costs_;
// Static layer parameters
bool trinary_costmap_;
bool track_unknown_space_;
int lethal_threshold_;
int unknown_cost_value_;
// Flags to determine the origin of the costmap
bool map_provided_;
bool using_test_map_;
};
} // namespace nav2_util
#endif // NAV2_UTIL__COSTMAP_HPP_
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_UTIL__EXECUTION_TIMER_HPP_
#define NAV2_UTIL__EXECUTION_TIMER_HPP_
#include <chrono>
namespace nav2_util
{
/// @brief Measures execution time of code between calls to start and end
24 class ExecutionTimer
{
public:
using Clock = std::chrono::high_resolution_clock;
using nanoseconds = std::chrono::nanoseconds;
/// @brief Call just prior to code you want to measure
void start( ) {start_ = Clock::now( );}
/// @brief Call just after the code you want to measure
void end( ) {end_ = Clock::now( );}
/// @brief Extract the measured time as an integral std::chrono::duration object
37 nanoseconds elapsed_time( ) {return end_ - start_;}
/// @brief Extract the measured time as a floating point number of seconds.
40 double elapsed_time_in_seconds( )
{
return std::chrono::duration<double>( end_ - start_ ).count( );
}
protected:
Clock::time_point start_;
Clock::time_point end_;
};
} // namespace nav2_util
#endif // NAV2_UTIL__EXECUTION_TIMER_HPP_
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_UTIL__GEOMETRY_UTILS_HPP_
#define NAV2_UTIL__GEOMETRY_UTILS_HPP_
#include <cmath>
#include "geometry_msgs/msg/pose.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/pose2_d.hpp"
#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "nav_msgs/msg/path.hpp"
namespace nav2_util
{
namespace geometry_utils
{
/**
* @brief Get a geometry_msgs Quaternion from a yaw angle
* @param angle Yaw angle to generate a quaternion from
* @return geometry_msgs Quaternion
*/
38 inline geometry_msgs::msg::Quaternion orientationAroundZAxis( double angle )
{
tf2::Quaternion q;
q.setRPY( 0, 0, angle ); // void returning function
return tf2::toMsg( q );
}
/**
* @brief Get the euclidean distance between 2 geometry_msgs::Points
* @param pos1 First point
* @param pos1 Second point
* @param is_3d True if a true L2 distance is desired ( default false )
* @return double L2 distance
*/
52 inline double euclidean_distance(
const geometry_msgs::msg::Point & pos1,
const geometry_msgs::msg::Point & pos2,
const bool is_3d = false )
{
double dx = pos1.x - pos2.x;
double dy = pos1.y - pos2.y;
if ( is_3d ) {
double dz = pos1.z - pos2.z;
return std::hypot( dx, dy, dz );
}
return std::hypot( dx, dy );
}
/**
* @brief Get the L2 distance between 2 geometry_msgs::Poses
* @param pos1 First pose
* @param pos1 Second pose
* @param is_3d True if a true L2 distance is desired ( default false )
* @return double euclidean distance
*/
75 inline double euclidean_distance(
const geometry_msgs::msg::Pose & pos1,
const geometry_msgs::msg::Pose & pos2,
const bool is_3d = false )
{
double dx = pos1.position.x - pos2.position.x;
double dy = pos1.position.y - pos2.position.y;
if ( is_3d ) {
double dz = pos1.position.z - pos2.position.z;
return std::hypot( dx, dy, dz );
}
return std::hypot( dx, dy );
}
/**
* @brief Get the L2 distance between 2 geometry_msgs::PoseStamped
* @param pos1 First pose
* @param pos1 Second pose
* @param is_3d True if a true L2 distance is desired ( default false )
* @return double L2 distance
*/
98 inline double euclidean_distance(
const geometry_msgs::msg::PoseStamped & pos1,
const geometry_msgs::msg::PoseStamped & pos2,
const bool is_3d = false )
{
return euclidean_distance( pos1.pose, pos2.pose, is_3d );
}
/**
* @brief Get the L2 distance between 2 geometry_msgs::Pose2D
* @param pos1 First pose
* @param pos1 Second pose
* @return double L2 distance
*/
112 inline double euclidean_distance(
const geometry_msgs::msg::Pose2D & pos1,
const geometry_msgs::msg::Pose2D & pos2 )
{
double dx = pos1.x - pos2.x;
double dy = pos1.y - pos2.y;
return std::hypot( dx, dy );
}
/**
* Find element in iterator with the minimum calculated value
*/
template<typename Iter, typename Getter>
126 inline Iter min_by( Iter begin, Iter end, Getter getCompareVal )
{
if ( begin == end ) {
return end;
}
auto lowest = getCompareVal( *begin );
Iter lowest_it = begin;
for ( Iter it = ++begin; it != end; ++it ) {
auto comp = getCompareVal( *it );
if ( comp < lowest ) {
lowest = comp;
lowest_it = it;
}
}
return lowest_it;
}
/**
* Find first element in iterator that is greater integrated distance than comparevalue
*/
template<typename Iter, typename Getter>
147 inline Iter first_after_integrated_distance( Iter begin, Iter end, Getter getCompareVal )
{
if ( begin == end ) {
return end;
}
Getter dist = 0.0;
for ( Iter it = begin; it != end - 1; it++ ) {
dist += euclidean_distance( *it, *( it + 1 ) );
if ( dist > getCompareVal ) {
return it + 1;
}
}
return end;
}
/**
* @brief Calculate the length of the provided path, starting at the provided index
* @param path Path containing the poses that are planned
* @param start_index Optional argument specifying the starting index for
* the calculation of path length. Provide this if you want to calculate length of a
* subset of the path.
* @return double Path length
*/
170 inline double calculate_path_length( const nav_msgs::msg::Path & path, size_t start_index = 0 )
{
if ( start_index + 1 >= path.poses.size( ) ) {
return 0.0;
}
double path_length = 0.0;
for ( size_t idx = start_index; idx < path.poses.size( ) - 1; ++idx ) {
path_length += euclidean_distance( path.poses[idx].pose, path.poses[idx + 1].pose );
}
return path_length;
}
} // namespace geometry_utils
} // namespace nav2_util
#endif // NAV2_UTIL__GEOMETRY_UTILS_HPP_
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_UTIL__LIFECYCLE_NODE_HPP_
#define NAV2_UTIL__LIFECYCLE_NODE_HPP_
#include <memory>
#include <string>
#include <thread>
#include "nav2_util/node_thread.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "rclcpp/rclcpp.hpp"
#include "bondcpp/bond.hpp"
#include "bond/msg/constants.hpp"
namespace nav2_util
{
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
/**
* @class nav2_util::LifecycleNode
* @brief A lifecycle node wrapper to enable common Nav2 needs such as manipulating parameters
*/
37 class LifecycleNode : public rclcpp_lifecycle::LifecycleNode
{
public:
/**
* @brief A lifecycle node constructor
* @param node_name Name for the node
* @param namespace Namespace for the node, if any
* @param options Node options
*/
46 LifecycleNode(
const std::string & node_name,
const std::string & ns = "",
const rclcpp::NodeOptions & options = rclcpp::NodeOptions( ) );
50 virtual ~LifecycleNode( );
typedef struct
{
double from_value;
double to_value;
double step;
} floating_point_range;
typedef struct
{
int from_value;
int to_value;
int step;
} integer_range;
/**
* @brief Declare a parameter that has no integer or floating point range constraints
* @param node_name Name of parameter
* @param default_value Default node value to add
* @param description Node description
* @param additional_constraints Any additional constraints on the parameters to list
* @param read_only Whether this param should be considered read only
*/
74 void add_parameter(
75 const std::string & name, const rclcpp::ParameterValue & default_value,
76 const std::string & description = "", const std::string & additional_constraints = "",
77 bool read_only = false )
{
auto descriptor = rcl_interfaces::msg::ParameterDescriptor( );
descriptor.name = name;
descriptor.description = description;
descriptor.additional_constraints = additional_constraints;
descriptor.read_only = read_only;
declare_parameter( descriptor.name, default_value, descriptor );
}
/**
* @brief Declare a parameter that has a floating point range constraint
* @param node_name Name of parameter
* @param default_value Default node value to add
* @param fp_range floating point range
* @param description Node description
* @param additional_constraints Any additional constraints on the parameters to list
* @param read_only Whether this param should be considered read only
*/
98 void add_parameter(
99 const std::string & name, const rclcpp::ParameterValue & default_value,
100 const floating_point_range fp_range,
101 const std::string & description = "", const std::string & additional_constraints = "",
102 bool read_only = false )
{
auto descriptor = rcl_interfaces::msg::ParameterDescriptor( );
descriptor.name = name;
descriptor.description = description;
descriptor.additional_constraints = additional_constraints;
descriptor.read_only = read_only;
descriptor.floating_point_range.resize( 1 );
descriptor.floating_point_range[0].from_value = fp_range.from_value;
descriptor.floating_point_range[0].to_value = fp_range.to_value;
descriptor.floating_point_range[0].step = fp_range.step;
declare_parameter( descriptor.name, default_value, descriptor );
}
/**
* @brief Declare a parameter that has an integer range constraint
* @param node_name Name of parameter
* @param default_value Default node value to add
* @param integer_range Integer range
* @param description Node description
* @param additional_constraints Any additional constraints on the parameters to list
* @param read_only Whether this param should be considered read only
*/
127 void add_parameter(
128 const std::string & name, const rclcpp::ParameterValue & default_value,
129 const integer_range int_range,
130 const std::string & description = "", const std::string & additional_constraints = "",
131 bool read_only = false )
{
auto descriptor = rcl_interfaces::msg::ParameterDescriptor( );
descriptor.name = name;
descriptor.description = description;
descriptor.additional_constraints = additional_constraints;
descriptor.read_only = read_only;
descriptor.integer_range.resize( 1 );
descriptor.integer_range[0].from_value = int_range.from_value;
descriptor.integer_range[0].to_value = int_range.to_value;
descriptor.integer_range[0].step = int_range.step;
declare_parameter( descriptor.name, default_value, descriptor );
}
/**
* @brief Get a shared pointer of this
*/
150 std::shared_ptr<nav2_util::LifecycleNode> shared_from_this( )
{
return std::static_pointer_cast<nav2_util::LifecycleNode>(
rclcpp_lifecycle::LifecycleNode::shared_from_this( ) );
}
/**
* @brief Abstracted on_error state transition callback, since unimplemented as of 2020
* in the managed ROS2 node state machine
* @param state State prior to error transition
* @return Return type for success or failed transition to error state
*/
162 nav2_util::CallbackReturn on_error( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_FATAL(
get_logger( ),
"Lifecycle node %s does not have error state implemented", get_name( ) );
return nav2_util::CallbackReturn::SUCCESS;
}
/**
* @brief Create bond connection to lifecycle manager
*/
173 void createBond( );
/**
* @brief Destroy bond connection to lifecycle manager
*/
178 void destroyBond( );
protected:
/**
* @brief Print notifications for lifecycle node
*/
184 void printLifecycleNodeNotification( );
// Connection to tell that server is still up
std::unique_ptr<bond::Bond> bond_{nullptr};
};
} // namespace nav2_util
#endif // NAV2_UTIL__LIFECYCLE_NODE_HPP_
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_UTIL__LIFECYCLE_SERVICE_CLIENT_HPP_
#define NAV2_UTIL__LIFECYCLE_SERVICE_CLIENT_HPP_
#include <chrono>
#include <memory>
#include <string>
#include "lifecycle_msgs/srv/change_state.hpp"
#include "lifecycle_msgs/srv/get_state.hpp"
#include "nav2_util/service_client.hpp"
#include "nav2_util/node_utils.hpp"
namespace nav2_util
{
/// Helper functions to interact with a lifecycle node.
31 class LifecycleServiceClient
{
public:
34 explicit LifecycleServiceClient( const std::string & lifecycle_node_name );
35 LifecycleServiceClient(
36 const std::string & lifecycle_node_name,
37 rclcpp::Node::SharedPtr parent_node );
/// Trigger a state change
/**
* Throws std::runtime_error on failure
*/
43 bool change_state(
44 const uint8_t transition, // takes a lifecycle_msgs::msg::Transition id
45 const std::chrono::seconds timeout );
/// Trigger a state change, returning result
48 bool change_state( std::uint8_t transition );
/// Get the current state as a lifecycle_msgs::msg::State id value
/**
* Throws std::runtime_error on failure
*/
54 uint8_t get_state( const std::chrono::seconds timeout = std::chrono::seconds( 2 ) );
protected:
57 rclcpp::Node::SharedPtr node_;
58 ServiceClient<lifecycle_msgs::srv::ChangeState> change_state_;
59 ServiceClient<lifecycle_msgs::srv::GetState> get_state_;
};
} // namespace nav2_util
#endif // NAV2_UTIL__LIFECYCLE_SERVICE_CLIENT_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_UTIL__LIFECYCLE_UTILS_HPP_
#define NAV2_UTIL__LIFECYCLE_UTILS_HPP_
#include <vector>
#include <string>
#include <chrono>
#include "nav2_util/string_utils.hpp"
namespace nav2_util
{
/// Transition the given lifecycle nodes to the ACTIVATED state in order
/** At this time, service calls frequently hang for unknown reasons. The only
* way to combat that is to timeout the service call and retry it. To use this
* function, estimate how long your nodes should take to at each transition and
* set your timeout accordingly.
* \param[in] node_names A vector of the fully qualified node names to startup.
* \param[in] service_call_timeout The maximum amount of time to wait for a
* service call.
* \param[in] retries The number of times to try a state transition service call
*/
36 void startup_lifecycle_nodes(
const std::vector<std::string> & node_names,
const std::chrono::seconds service_call_timeout = std::chrono::seconds::max( ),
const int retries = 3 );
/// Transition the given lifecycle nodes to the ACTIVATED state in order.
/**
* \param[in] nodes A ':' seperated list of node names. eg. "/node1:/node2"
*/
45 void startup_lifecycle_nodes(
const std::string & nodes,
const std::chrono::seconds service_call_timeout = std::chrono::seconds::max( ),
const int retries = 3 )
{
startup_lifecycle_nodes( split( nodes, ':' ), service_call_timeout, retries );
}
/// Transition the given lifecycle nodes to the UNCONFIGURED state in order
/** At this time, service calls frequently hang for unknown reasons. The only
* way to combat that is to timeout the service call and retry it. To use this
* function, estimate how long your nodes should take to at each transition and
* set your timeout accordingly.
* \param[in] node_names A vector of the fully qualified node names to reset.
* \param[in] service_call_timeout The maximum amount of time to wait for a
* service call.
* \param[in] retries The number of times to try a state transition service call
*/
63 void reset_lifecycle_nodes(
const std::vector<std::string> & node_names,
const std::chrono::seconds service_call_timeout = std::chrono::seconds::max( ),
const int retries = 3 );
/// Transition the given lifecycle nodes to the UNCONFIGURED state in order.
/**
* \param[in] nodes A ':' seperated list of node names. eg. "/node1:/node2"
*/
72 void reset_lifecycle_nodes(
const std::string & nodes,
const std::chrono::seconds service_call_timeout = std::chrono::seconds::max( ),
const int retries = 3 )
{
reset_lifecycle_nodes( split( nodes, ':' ), service_call_timeout, retries );
}
} // namespace nav2_util
#endif // NAV2_UTIL__LIFECYCLE_UTILS_HPP_
1 // Copyright ( c ) 2012, Willow Garage, Inc.
// All rights reserved.
//
// Software License Agreement ( BSD License 2.0 )
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above
// copyright notice, this list of conditions and the following
// disclaimer in the documentation and/or other materials provided
// with the distribution.
// * Neither the name of the Willow Garage, Inc. nor the names of its
// contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
// LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
#ifndef NAV2_UTIL__LINE_ITERATOR_HPP_
#define NAV2_UTIL__LINE_ITERATOR_HPP_
#include <stdlib.h>
namespace nav2_util
{
/**
* @class nav2_util::LineIterator
* @brief An iterator implementing Bresenham Ray-Tracing.
*/
46 class LineIterator
{
public:
/**
* @brief A constructor for LineIterator
* @param x0 Starting x
* @param y0 Starting y
* @param x1 Ending x
* @param y1 Ending y
*/
56 LineIterator( int x0, int y0, int x1, int y1 )
: x0_( x0 ),
y0_( y0 ),
x1_( x1 ),
y1_( y1 ),
x_( x0 ), // X and Y start of at first endpoint.
y_( y0 ),
deltax_( abs( x1 - x0 ) ),
deltay_( abs( y1 - y0 ) ),
curpixel_( 0 )
{
if ( x1_ >= x0_ ) { // The x-values are increasing
xinc1_ = 1;
xinc2_ = 1;
} else { // The x-values are decreasing
xinc1_ = -1;
xinc2_ = -1;
}
if ( y1_ >= y0_ ) { // The y-values are increasing
yinc1_ = 1;
yinc2_ = 1;
} else { // The y-values are decreasing
yinc1_ = -1;
yinc2_ = -1;
}
if ( deltax_ >= deltay_ ) { // There is at least one x-value for every y-value
xinc1_ = 0; // Don't change the x when numerator >= denominator
yinc2_ = 0; // Don't change the y for every iteration
den_ = deltax_;
num_ = deltax_ / 2;
numadd_ = deltay_;
numpixels_ = deltax_; // There are more x-values than y-values
} else { // There is at least one y-value for every x-value
xinc2_ = 0; // Don't change the x for every iteration
yinc1_ = 0; // Don't change the y when numerator >= denominator
den_ = deltay_;
num_ = deltay_ / 2;
numadd_ = deltax_;
numpixels_ = deltay_; // There are more y-values than x-values
}
}
/**
* @brief If the iterator is valid
* @return bool If valid
*/
104 bool isValid( ) const
{
return curpixel_ <= numpixels_;
}
/**
* @brief Advance iteration along the line
*/
112 void advance( )
{
num_ += numadd_; // Increase the numerator by the top of the fraction
if ( num_ >= den_ ) { // Check if numerator >= denominator
num_ -= den_; // Calculate the new numerator value
x_ += xinc1_; // Change the x as appropriate
y_ += yinc1_; // Change the y as appropriate
}
x_ += xinc2_; // Change the x as appropriate
y_ += yinc2_; // Change the y as appropriate
curpixel_++;
}
/**
* @brief Get current X value
* @return X
*/
130 int getX( ) const
{
return x_;
}
/**
* @brief Get current Y value
* @return Y
*/
139 int getY( ) const
{
return y_;
}
/**
* @brief Get initial X value
* @return X
*/
148 int getX0( ) const
{
return x0_;
}
/**
* @brief Get initial Y value
* @return Y
*/
157 int getY0( ) const
{
return y0_;
}
/**
* @brief Get terminal X value
* @return X
*/
166 int getX1( ) const
{
return x1_;
}
/**
* @brief Get terminal Y value
* @return Y
*/
175 int getY1( ) const
{
return y1_;
}
private:
int x0_; ///< X coordinate of first end point.
int y0_; ///< Y coordinate of first end point.
int x1_; ///< X coordinate of second end point.
int y1_; ///< Y coordinate of second end point.
int x_; ///< X coordinate of current point.
int y_; ///< Y coordinate of current point.
int deltax_; ///< Difference between Xs of endpoints.
int deltay_; ///< Difference between Ys of endpoints.
int curpixel_; ///< index of current point in line loop.
int xinc1_, xinc2_, yinc1_, yinc2_;
int den_, num_, numadd_, numpixels_;
};
} // end namespace nav2_util
#endif // NAV2_UTIL__LINE_ITERATOR_HPP_
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_UTIL__NODE_THREAD_HPP_
#define NAV2_UTIL__NODE_THREAD_HPP_
#include <memory>
#include "rclcpp/rclcpp.hpp"
namespace nav2_util
{
/**
* @class nav2_util::NodeThread
* @brief A background thread to process node/executor callbacks
*/
28 class NodeThread
{
public:
/**
* @brief A background thread to process node callbacks constructor
* @param node_base Interface to Node to spin in thread
*/
35 explicit NodeThread( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base );
/**
* @brief A background thread to process executor's callbacks constructor
* @param executor Interface to executor to spin in thread
*/
41 explicit NodeThread( rclcpp::executors::SingleThreadedExecutor::SharedPtr executor );
/**
* @brief A background thread to process node callbacks constructor
* @param node Node pointer to spin in thread
*/
template<typename NodeT>
explicit NodeThread( NodeT node )
: NodeThread( node->get_node_base_interface( ) )
{}
/**
* @brief A destructor
*/
~NodeThread( );
protected:
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_;
std::unique_ptr<std::thread> thread_;
rclcpp::Executor::SharedPtr executor_;
};
} // namespace nav2_util
#endif // NAV2_UTIL__NODE_THREAD_HPP_
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_UTIL__NODE_UTILS_HPP_
#define NAV2_UTIL__NODE_UTILS_HPP_
#include <string>
#include "rclcpp/rclcpp.hpp"
namespace nav2_util
{
/// Replace invalid characters in a potential node name
/**
* There is frequently a need to create internal nodes. They must have a name,
* and commonly the name is based on some parameter related to the node's
* purpose. However, only alphanumeric characters and '_' are allowed in node
* names. This function replaces any invalid character with a '_'
*
* \param[in] potential_node_name Potential name but possibly with invalid charaters.
* \return A copy of the input string but with non-alphanumeric characters replaced with '_'
*/
34 std::string sanitize_node_name( const std::string & potential_node_name );
/// Concatenate two namespaces to produce an absolute namespace
/**
* \param[in] top_ns The namespace to place first
* \param[in] sub_ns The namespace to place after top_ns
* \return An absolute namespace starting with "/"
*/
42 std::string add_namespaces( const std::string & top_ns, const std::string & sub_ns = "" );
/// Add some random characters to a node name to ensure it is unique in the system
/**
* There are utility classes that create an internal private node to interact
* with the system. These private nodes are given a generated name. If multiple
* clients end up using the same service, there is the potential for node name
* conflicts. To ensure node names are globally unique, this appends some random
* numbers to the end of the prefix.
*
* \param[in] prefix A string to help understand the purpose of the node.
* \return A copy of the prefix + '_' + 8 random digits. eg. prefix_12345678
*/
55 std::string generate_internal_node_name( const std::string & prefix = "" );
/// Creates a node with a name as generated by generate_internal_node_name
/**
* Creates a node with the following settings:
* - name generated by generate_internal_node_name
* - no parameter services
* - no parameter event publisher
*
* \param[in] prefix A string to help understand the purpose of the node.
* \return A shared_ptr to the node.
*/
67 rclcpp::Node::SharedPtr generate_internal_node( const std::string & prefix = "" );
/// Generates a pseudo random string of digits.
/**
* Generates pseudo random digits by converting the current system time to a
* string. This means that any length more than 8 or so digits will just get
* padded with zeros and doesn't add any additional randomness.
*
* \param[in] len Length of the output string
* \return A string containing random digits
*/
78 std::string time_to_string( size_t len );
rclcpp::NodeOptions
81 get_node_options_default( bool allow_undeclared = true, bool declare_initial_params = true );
/// Declares static ROS2 parameter and sets it to a given value if it was not already declared
/* Declares static ROS2 parameter and sets it to a given value
* if it was not already declared.
*
* \param[in] node A node in which given parameter to be declared
* \param[in] param_name The name of parameter
* \param[in] default_value Parameter value to initialize with
* \param[in] parameter_descriptor Parameter descriptor ( optional )
*/
template<typename NodeT>
93 void declare_parameter_if_not_declared(
NodeT node,
const std::string & param_name,
const rclcpp::ParameterValue & default_value,
const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
rcl_interfaces::msg::ParameterDescriptor( ) )
{
if ( !node->has_parameter( param_name ) ) {
node->declare_parameter( param_name, default_value, parameter_descriptor );
}
}
/// Declares static ROS2 parameter with given type if it was not already declared
/* Declares static ROS2 parameter with given type if it was not already declared.
*
* \param[in] node A node in which given parameter to be declared
* \param[in] param_type The type of parameter
* \param[in] default_value Parameter value to initialize with
* \param[in] parameter_descriptor Parameter descriptor ( optional )
*/
template<typename NodeT>
114 void declare_parameter_if_not_declared(
NodeT node,
const std::string & param_name,
const rclcpp::ParameterType & param_type,
const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
rcl_interfaces::msg::ParameterDescriptor( ) )
{
if ( !node->has_parameter( param_name ) ) {
node->declare_parameter( param_name, param_type, parameter_descriptor );
}
}
/// Gets the type of plugin for the selected node and its plugin
/**
* Gets the type of plugin for the selected node and its plugin.
* Actually seeks for the value of "<plugin_name>.plugin" parameter.
*
* \param[in] node Selected node
* \param[in] plugin_name The name of plugin the type of which is being searched for
* \return A string containing the type of plugin ( the value of "<plugin_name>.plugin" parameter )
*/
template<typename NodeT>
136 std::string get_plugin_type_param(
NodeT node,
const std::string & plugin_name )
{
declare_parameter_if_not_declared( node, plugin_name + ".plugin", rclcpp::PARAMETER_STRING );
std::string plugin_type;
try {
if ( !node->get_parameter( plugin_name + ".plugin", plugin_type ) ) {
RCLCPP_FATAL(
node->get_logger( ), "Can not get 'plugin' param value for %s", plugin_name.c_str( ) );
exit( -1 );
}
} catch ( rclcpp::exceptions::ParameterUninitializedException & ex ) {
RCLCPP_FATAL( node->get_logger( ), "'plugin' param not defined for %s", plugin_name.c_str( ) );
exit( -1 );
}
return plugin_type;
}
} // namespace nav2_util
#endif // NAV2_UTIL__NODE_UTILS_HPP_
1 // Copyright ( c ) 2020 Samsung Research Russia
// All rights reserved.
//
// Software License Agreement ( BSD License 2.0 )
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above
// copyright notice, this list of conditions and the following
// disclaimer in the documentation and/or other materials provided
// with the distribution.
// * Neither the name of the <ORGANIZATION> nor the names of its
// contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
// LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: Alexey Merzlyakov
#ifndef NAV2_UTIL__OCC_GRID_VALUES_HPP_
#define NAV2_UTIL__OCC_GRID_VALUES_HPP_
namespace nav2_util
{
/**
* @brief OccupancyGrid data constants
*/
static constexpr int8_t OCC_GRID_UNKNOWN = -1;
static constexpr int8_t OCC_GRID_FREE = 0;
static constexpr int8_t OCC_GRID_OCCUPIED = 100;
} // namespace nav2_util
#endif // NAV2_UTIL__OCC_GRID_VALUES_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_UTIL__ODOMETRY_UTILS_HPP_
#define NAV2_UTIL__ODOMETRY_UTILS_HPP_
#include <cmath>
#include <chrono>
#include <memory>
#include <mutex>
#include <string>
#include <deque>
#include "geometry_msgs/msg/twist.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "rclcpp/rclcpp.hpp"
#include "nav2_util/node_utils.hpp"
namespace nav2_util
{
/**
* @class OdomSmoother
* Wrapper for getting smooth odometry readings using a simple moving avergae.
* Subscribes to the topic with a mutex.
*/
41 class OdomSmoother
{
public:
/**
* @brief Constructor that subscribes to an Odometry topic
* @param parent NodeHandle for creating subscriber
* @param filter_duration Duration for odom history ( seconds )
* @param odom_topic Topic on which odometry should be received
*/
50 explicit OdomSmoother(
51 const rclcpp::Node::WeakPtr & parent,
double filter_duration = 0.3,
53 const std::string & odom_topic = "odom" );
/**
* @brief Overloadded Constructor for nav_util::LifecycleNode parent
* that subscribes to an Odometry topic
* @param parent NodeHandle for creating subscriber
* @param filter_duration Duration for odom history ( seconds )
* @param odom_topic Topic on which odometry should be received
*/
62 explicit OdomSmoother(
63 const nav2_util::LifecycleNode::WeakPtr & parent,
double filter_duration = 0.3,
65 const std::string & odom_topic = "odom" );
/**
* @brief Get twist msg from smoother
* @return twist Twist msg
*/
71 inline geometry_msgs::msg::Twist getTwist( ) {return vel_smooth_.twist;}
/**
* @brief Get twist stamped msg from smoother
* @return twist TwistStamped msg
*/
77 inline geometry_msgs::msg::TwistStamped getTwistStamped( ) {return vel_smooth_;}
protected:
/**
* @brief Callback of odometry subscriber to process
* @param msg Odometry msg to smooth
*/
84 void odomCallback( nav_msgs::msg::Odometry::SharedPtr msg );
/**
* @brief Update internal state of the smoother after getting new data
*/
89 void updateState( );
91 rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
92 nav_msgs::msg::Odometry odom_cumulate_;
93 geometry_msgs::msg::TwistStamped vel_smooth_;
94 std::mutex odom_mutex_;
96 rclcpp::Duration odom_history_duration_;
97 std::deque<nav_msgs::msg::Odometry> odom_history_;
};
} // namespace nav2_util
#endif // NAV2_UTIL__ODOMETRY_UTILS_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2019 Steven Macenski
// Copyright ( c ) 2019 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_UTIL__ROBOT_UTILS_HPP_
#define NAV2_UTIL__ROBOT_UTILS_HPP_
#include <string>
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "rclcpp/rclcpp.hpp"
namespace nav2_util
{
/**
* @brief get the current pose of the robot
* @param global_pose Pose to transform
* @param tf_buffer TF buffer to use for the transformation
* @param global_frame Frame to transform into
* @param robot_frame Frame to transform from
* @param transform_timeout TF Timeout to use for transformation
* @return bool Whether it could be transformed successfully
*/
38 bool getCurrentPose(
geometry_msgs::msg::PoseStamped & global_pose,
tf2_ros::Buffer & tf_buffer, const std::string global_frame = "map",
const std::string robot_frame = "base_link", const double transform_timeout = 0.1,
const rclcpp::Time stamp = rclcpp::Time( ) );
/**
* @brief get an arbitrary pose in a target frame
* @param input_pose Pose to transform
* @param transformed_pose Output transformation
* @param tf_buffer TF buffer to use for the transformation
* @param target_frame Frame to transform into
* @param transform_timeout TF Timeout to use for transformation
* @return bool Whether it could be transformed successfully
*/
53 bool transformPoseInTargetFrame(
const geometry_msgs::msg::PoseStamped & input_pose,
geometry_msgs::msg::PoseStamped & transformed_pose,
tf2_ros::Buffer & tf_buffer, const std::string target_frame,
const double transform_timeout = 0.1 );
} // end namespace nav2_util
#endif // NAV2_UTIL__ROBOT_UTILS_HPP_
// Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_UTIL__SERVICE_CLIENT_HPP_
#define NAV2_UTIL__SERVICE_CLIENT_HPP_
#include <string>
#include "rclcpp/rclcpp.hpp"
namespace nav2_util
{
/**
* @class nav2_util::ServiceClient
* @brief A simple wrapper on ROS2 services for invoke( ) and block-style calling
*/
template<class ServiceT>
30 class ServiceClient
{
public:
/**
* @brief A constructor
* @param service_name name of the service to call
* @param provided_node Node to create the service client off of
*/
38 explicit ServiceClient(
39 const std::string & service_name,
40 const rclcpp::Node::SharedPtr & provided_node )
: service_name_( service_name ), node_( provided_node )
{
callback_group_ = node_->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive,
false );
callback_group_executor_.add_callback_group( callback_group_, node_->get_node_base_interface( ) );
client_ = node_->create_client<ServiceT>(
service_name,
rmw_qos_profile_services_default,
callback_group_ );
}
using RequestType = typename ServiceT::Request;
using ResponseType = typename ServiceT::Response;
/**
* @brief Invoke the service and block until completed or timed out
* @param request The request object to call the service using
* @param timeout Maximum timeout to wait for, default infinite
* @return Response A pointer to the service response from the request
*/
typename ResponseType::SharedPtr invoke(
typename RequestType::SharedPtr & request,
const std::chrono::nanoseconds timeout = std::chrono::nanoseconds( -1 ) )
{
while ( !client_->wait_for_service( std::chrono::seconds( 1 ) ) ) {
if ( !rclcpp::ok( ) ) {
throw std::runtime_error(
service_name_ + " service client: interrupted while waiting for service" );
}
RCLCPP_INFO(
72 node_->get_logger( ), "%s service client: waiting for service to appear...",
service_name_.c_str( ) );
}
RCLCPP_DEBUG(
node_->get_logger( ), "%s service client: send async request",
service_name_.c_str( ) );
auto future_result = client_->async_send_request( request );
if ( callback_group_executor_.spin_until_future_complete( future_result, timeout ) !=
rclcpp::FutureReturnCode::SUCCESS )
{
throw std::runtime_error( service_name_ + " service client: async_send_request failed" );
}
return future_result.get( );
}
/**
* @brief Invoke the service and block until completed
* @param request The request object to call the service using
* @param Response A pointer to the service response from the request
* @return bool Whether it was successfully called
*/
bool invoke(
typename RequestType::SharedPtr & request,
typename ResponseType::SharedPtr & response )
{
while ( !client_->wait_for_service( std::chrono::seconds( 1 ) ) ) {
if ( !rclcpp::ok( ) ) {
throw std::runtime_error(
service_name_ + " service client: interrupted while waiting for service" );
}
RCLCPP_INFO(
node_->get_logger( ), "%s service client: waiting for service to appear...",
service_name_.c_str( ) );
}
RCLCPP_DEBUG(
node_->get_logger( ), "%s service client: send async request",
service_name_.c_str( ) );
auto future_result = client_->async_send_request( request );
if ( callback_group_executor_.spin_until_future_complete( future_result ) !=
rclcpp::FutureReturnCode::SUCCESS )
{
return false;
}
response = future_result.get( );
return response.get( );
}
/**
* @brief Block until a service is available or timeout
* @param timeout Maximum timeout to wait for, default infinite
* @return bool true if service is available
*/
bool wait_for_service( const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::max( ) )
{
return client_->wait_for_service( timeout );
}
protected:
std::string service_name_;
rclcpp::Node::SharedPtr node_;
rclcpp::CallbackGroup::SharedPtr callback_group_;
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
typename rclcpp::Client<ServiceT>::SharedPtr client_;
};
} // namespace nav2_util
#endif // NAV2_UTIL__SERVICE_CLIENT_HPP_
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_UTIL__SIMPLE_ACTION_SERVER_HPP_
#define NAV2_UTIL__SIMPLE_ACTION_SERVER_HPP_
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <future>
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_util/node_thread.hpp"
namespace nav2_util
{
/**
* @class nav2_util::SimpleActionServer
* @brief An action server wrapper to make applications simpler using Actions
*/
template<typename ActionT>
37 class SimpleActionServer
{
public:
// Callback function to complete main work. This should itself deal with its
// own exceptions, but if for some reason one is thrown, it will be caught
// in SimpleActionServer and terminate the action itself.
typedef std::function<void ( )> ExecuteCallback;
// Callback function to notify the user that an exception was thrown that
// the simple action server caught ( or another failure ) and the action was
// terminated. To avoid using, catch exceptions in your application such that
// the SimpleActionServer will never need to terminate based on failed action
// ExecuteCallback.
typedef std::function<void ( )> CompletionCallback;
/**
* @brief An constructor for SimpleActionServer
* @param node Ptr to node to make actions
* @param action_name Name of the action to call
* @param execute_callback Execution callback function of Action
* @param server_timeout Timeout to to react to stop or preemption requests
* @param spin_thread Whether to spin with a dedicated thread internally
* @param options Options to pass to the underlying rcl_action_server_t
*/
template<typename NodeT>
explicit SimpleActionServer(
NodeT node,
const std::string & action_name,
ExecuteCallback execute_callback,
CompletionCallback completion_callback = nullptr,
std::chrono::milliseconds server_timeout = std::chrono::milliseconds( 500 ),
bool spin_thread = false,
const rcl_action_server_options_t & options = rcl_action_server_get_default_options( ) )
: SimpleActionServer(
node->get_node_base_interface( ),
node->get_node_clock_interface( ),
node->get_node_logging_interface( ),
node->get_node_waitables_interface( ),
action_name, execute_callback, completion_callback, server_timeout, spin_thread, options )
{}
/**
* @brief An constructor for SimpleActionServer
* @param <node interfaces> Abstract node interfaces to make actions
* @param action_name Name of the action to call
* @param execute_callback Execution callback function of Action
* @param server_timeout Timeout to to react to stop or preemption requests
* @param spin_thread Whether to spin with a dedicated thread internally
* @param options Options to pass to the underlying rcl_action_server_t
*/
87 explicit SimpleActionServer(
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface,
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface,
rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface,
const std::string & action_name,
ExecuteCallback execute_callback,
CompletionCallback completion_callback = nullptr,
std::chrono::milliseconds server_timeout = std::chrono::milliseconds( 500 ),
bool spin_thread = false,
const rcl_action_server_options_t & options = rcl_action_server_get_default_options( ) )
: node_base_interface_( node_base_interface ),
node_clock_interface_( node_clock_interface ),
node_logging_interface_( node_logging_interface ),
node_waitables_interface_( node_waitables_interface ),
action_name_( action_name ),
execute_callback_( execute_callback ),
completion_callback_( completion_callback ),
server_timeout_( server_timeout ),
spin_thread_( spin_thread )
{
using namespace std::placeholders; // NOLINT
if ( spin_thread_ ) {
callback_group_ = node_base_interface->create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive, false );
}
action_server_ = rclcpp_action::create_server<ActionT>(
node_base_interface_,
node_clock_interface_,
node_logging_interface_,
node_waitables_interface_,
action_name_,
std::bind( &SimpleActionServer::handle_goal, this, _1, _2 ),
std::bind( &SimpleActionServer::handle_cancel, this, _1 ),
std::bind( &SimpleActionServer::handle_accepted, this, _1 ),
options,
callback_group_ );
if ( spin_thread_ ) {
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>( );
executor_->add_callback_group( callback_group_, node_base_interface_ );
executor_thread_ = std::make_unique<nav2_util::NodeThread>( executor_ );
}
}
/**
* @brief handle the goal requested: accept or reject. This implementation always accepts.
* @param uuid Goal ID
* @param Goal A shared pointer to the specific goal
* @return GoalResponse response of the goal processed
*/
137 rclcpp_action::GoalResponse handle_goal(
const rclcpp_action::GoalUUID & /*uuid*/,
std::shared_ptr<const typename ActionT::Goal>/*goal*/ )
{
std::lock_guard<std::recursive_mutex> lock( update_mutex_ );
if ( !server_active_ ) {
return rclcpp_action::GoalResponse::REJECT;
}
debug_msg( "Received request for goal acceptance" );
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
/**
* @brief Accepts cancellation requests of action server.
* @param uuid Goal ID
* @param Goal A server goal handle to cancel
* @return CancelResponse response of the goal cancelled
*/
157 rclcpp_action::CancelResponse handle_cancel(
const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> handle )
{
std::lock_guard<std::recursive_mutex> lock( update_mutex_ );
if ( !handle->is_active( ) ) {
warn_msg(
"Received request for goal cancellation, "
"but the handle is inactive, so reject the request" );
return rclcpp_action::CancelResponse::REJECT;
}
debug_msg( "Received request for goal cancellation" );
return rclcpp_action::CancelResponse::ACCEPT;
}
/**
* @brief Handles accepted goals and adds to preempted queue to switch to
* @param Goal A server goal handle to cancel
*/
177 void handle_accepted( const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> handle )
{
std::lock_guard<std::recursive_mutex> lock( update_mutex_ );
debug_msg( "Receiving a new goal" );
if ( is_active( current_handle_ ) || is_running( ) ) {
debug_msg( "An older goal is active, moving the new goal to a pending slot." );
if ( is_active( pending_handle_ ) ) {
debug_msg(
"The pending slot is occupied."
" The previous pending goal will be terminated and replaced." );
terminate( pending_handle_ );
}
pending_handle_ = handle;
preempt_requested_ = true;
} else {
if ( is_active( pending_handle_ ) ) {
// Shouldn't reach a state with a pending goal but no current one.
error_msg( "Forgot to handle a preemption. Terminating the pending goal." );
terminate( pending_handle_ );
preempt_requested_ = false;
}
current_handle_ = handle;
// Return quickly to avoid blocking the executor, so spin up a new thread
debug_msg( "Executing goal asynchronously." );
execution_future_ = std::async( std::launch::async, [this]( ) {work( );} );
}
}
/**
* @brief Computed background work and processes stop requests
*/
212 void work( )
{
while ( rclcpp::ok( ) && !stop_execution_ && is_active( current_handle_ ) ) {
debug_msg( "Executing the goal..." );
try {
execute_callback_( );
} catch ( std::exception & ex ) {
RCLCPP_ERROR(
node_logging_interface_->get_logger( ),
"Action server failed while executing action callback: \"%s\"", ex.what( ) );
terminate_all( );
completion_callback_( );
return;
}
debug_msg( "Blocking processing of new goal handles." );
std::lock_guard<std::recursive_mutex> lock( update_mutex_ );
if ( stop_execution_ ) {
warn_msg( "Stopping the thread per request." );
terminate_all( );
completion_callback_( );
break;
}
if ( is_active( current_handle_ ) ) {
warn_msg( "Current goal was not completed successfully." );
terminate( current_handle_ );
completion_callback_( );
}
if ( is_active( pending_handle_ ) ) {
debug_msg( "Executing a pending handle on the existing thread." );
accept_pending_goal( );
} else {
debug_msg( "Done processing available goals." );
break;
}
}
debug_msg( "Worker thread done." );
}
/**
* @brief Active action server
*/
257 void activate( )
{
std::lock_guard<std::recursive_mutex> lock( update_mutex_ );
server_active_ = true;
stop_execution_ = false;
}
/**
* @brief Deactive action server
*/
267 void deactivate( )
{
debug_msg( "Deactivating..." );
{
std::lock_guard<std::recursive_mutex> lock( update_mutex_ );
server_active_ = false;
stop_execution_ = true;
}
if ( !execution_future_.valid( ) ) {
return;
}
if ( is_running( ) ) {
warn_msg(
"Requested to deactivate server but goal is still executing."
" Should check if action server is running before deactivating." );
}
using namespace std::chrono; //NOLINT
auto start_time = steady_clock::now( );
while ( execution_future_.wait_for( milliseconds( 100 ) ) != std::future_status::ready ) {
info_msg( "Waiting for async process to finish." );
if ( steady_clock::now( ) - start_time >= server_timeout_ ) {
terminate_all( );
completion_callback_( );
throw std::runtime_error( "Action callback is still running and missed deadline to stop" );
}
}
debug_msg( "Deactivation completed." );
}
/**
* @brief Whether the action server is munching on a goal
* @return bool If its running or not
*/
305 bool is_running( )
{
return execution_future_.valid( ) &&
( execution_future_.wait_for( std::chrono::milliseconds( 0 ) ) ==
std::future_status::timeout );
}
/**
* @brief Whether the action server is active or not
* @return bool If its active or not
*/
316 bool is_server_active( )
{
std::lock_guard<std::recursive_mutex> lock( update_mutex_ );
return server_active_;
}
/**
* @brief Whether the action server has been asked to be preempted with a new goal
* @return bool If there's a preemption request or not
*/
326 bool is_preempt_requested( ) const
{
std::lock_guard<std::recursive_mutex> lock( update_mutex_ );
return preempt_requested_;
}
/**
* @brief Accept pending goals
* @return Goal Ptr to the goal that's going to be accepted
*/
336 const std::shared_ptr<const typename ActionT::Goal> accept_pending_goal( )
{
std::lock_guard<std::recursive_mutex> lock( update_mutex_ );
if ( !pending_handle_ || !pending_handle_->is_active( ) ) {
error_msg( "Attempting to get pending goal when not available" );
return std::shared_ptr<const typename ActionT::Goal>( );
}
if ( is_active( current_handle_ ) && current_handle_ != pending_handle_ ) {
debug_msg( "Cancelling the previous goal" );
current_handle_->abort( empty_result( ) );
}
current_handle_ = pending_handle_;
pending_handle_.reset( );
preempt_requested_ = false;
debug_msg( "Preempted goal" );
return current_handle_->get_goal( );
}
/**
* @brief Terminate pending goals
*/
362 void terminate_pending_goal( )
{
std::lock_guard<std::recursive_mutex> lock( update_mutex_ );
if ( !pending_handle_ || !pending_handle_->is_active( ) ) {
error_msg( "Attempting to terminate pending goal when not available" );
return;
}
terminate( pending_handle_ );
preempt_requested_ = false;
debug_msg( "Pending goal terminated" );
}
/**
* @brief Get the current goal object
* @return Goal Ptr to the goal that's being processed currently
*/
381 const std::shared_ptr<const typename ActionT::Goal> get_current_goal( ) const
{
std::lock_guard<std::recursive_mutex> lock( update_mutex_ );
if ( !is_active( current_handle_ ) ) {
error_msg( "A goal is not available or has reached a final state" );
return std::shared_ptr<const typename ActionT::Goal>( );
}
return current_handle_->get_goal( );
}
/**
* @brief Get the pending goal object
* @return Goal Ptr to the goal that's pending
*/
397 const std::shared_ptr<const typename ActionT::Goal> get_pending_goal( ) const
{
std::lock_guard<std::recursive_mutex> lock( update_mutex_ );
if ( !pending_handle_ || !pending_handle_->is_active( ) ) {
error_msg( "Attempting to get pending goal when not available" );
return std::shared_ptr<const typename ActionT::Goal>( );
}
return pending_handle_->get_goal( );
}
/**
* @brief Whether or not a cancel command has come in
* @return bool Whether a cancel command has been requested or not
*/
413 bool is_cancel_requested( ) const
{
std::lock_guard<std::recursive_mutex> lock( update_mutex_ );
// A cancel request is assumed if either handle is canceled by the client.
if ( current_handle_ == nullptr ) {
error_msg( "Checking for cancel but current goal is not available" );
return false;
}
if ( pending_handle_ != nullptr ) {
return pending_handle_->is_canceling( );
}
return current_handle_->is_canceling( );
}
/**
* @brief Terminate all pending and active actions
* @param result A result object to send to the terminated actions
*/
435 void terminate_all(
typename std::shared_ptr<typename ActionT::Result> result =
std::make_shared<typename ActionT::Result>( ) )
{
std::lock_guard<std::recursive_mutex> lock( update_mutex_ );
terminate( current_handle_, result );
terminate( pending_handle_, result );
preempt_requested_ = false;
}
/**
* @brief Terminate the active action
* @param result A result object to send to the terminated action
*/
449 void terminate_current(
typename std::shared_ptr<typename ActionT::Result> result =
std::make_shared<typename ActionT::Result>( ) )
{
std::lock_guard<std::recursive_mutex> lock( update_mutex_ );
terminate( current_handle_, result );
}
/**
* @brief Return success of the active action
* @param result A result object to send to the terminated actions
*/
461 void succeeded_current(
typename std::shared_ptr<typename ActionT::Result> result =
std::make_shared<typename ActionT::Result>( ) )
{
std::lock_guard<std::recursive_mutex> lock( update_mutex_ );
if ( is_active( current_handle_ ) ) {
debug_msg( "Setting succeed on current goal." );
current_handle_->succeed( result );
current_handle_.reset( );
}
}
/**
* @brief Publish feedback to the action server clients
* @param feedback A feedback object to send to the clients
*/
478 void publish_feedback( typename std::shared_ptr<typename ActionT::Feedback> feedback )
{
if ( !is_active( current_handle_ ) ) {
error_msg( "Trying to publish feedback when the current goal handle is not active" );
return;
}
current_handle_->publish_feedback( feedback );
}
protected:
// The SimpleActionServer isn't itself a node, so it needs interfaces to one
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_;
rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_interface_;
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_;
rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_interface_;
std::string action_name_;
ExecuteCallback execute_callback_;
CompletionCallback completion_callback_;
std::future<void> execution_future_;
bool stop_execution_{false};
mutable std::recursive_mutex update_mutex_;
bool server_active_{false};
bool preempt_requested_{false};
std::chrono::milliseconds server_timeout_;
std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> current_handle_;
std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> pending_handle_;
typename rclcpp_action::Server<ActionT>::SharedPtr action_server_;
bool spin_thread_;
rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr};
rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
std::unique_ptr<nav2_util::NodeThread> executor_thread_;
/**
* @brief Generate an empty result object for an action type
*/
constexpr auto empty_result( ) const
{
return std::make_shared<typename ActionT::Result>( );
}
/**
* @brief Whether a given goal handle is currently active
* @param handle Goal handle to check
* @return Whether this goal handle is active
*/
constexpr bool is_active(
const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> handle ) const
{
return handle != nullptr && handle->is_active( );
}
/**
* @brief Terminate a particular action with a result
* @param handle goal handle to terminate
* @param the Results object to terminate the action with
*/
void terminate(
std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> handle,
typename std::shared_ptr<typename ActionT::Result> result =
std::make_shared<typename ActionT::Result>( ) )
{
std::lock_guard<std::recursive_mutex> lock( update_mutex_ );
if ( is_active( handle ) ) {
if ( handle->is_canceling( ) ) {
warn_msg( "Client requested to cancel the goal. Cancelling." );
handle->canceled( result );
} else {
warn_msg( "Aborting handle." );
handle->abort( result );
}
handle.reset( );
}
}
/**
* @brief Info logging
*/
void info_msg( const std::string & msg ) const
{
RCLCPP_INFO(
node_logging_interface_->get_logger( ),
"[%s] [ActionServer] %s", action_name_.c_str( ), msg.c_str( ) );
}
/**
* @brief Debug logging
*/
void debug_msg( const std::string & msg ) const
{
RCLCPP_DEBUG(
node_logging_interface_->get_logger( ),
"[%s] [ActionServer] %s", action_name_.c_str( ), msg.c_str( ) );
}
/**
* @brief Error logging
*/
void error_msg( const std::string & msg ) const
{
RCLCPP_ERROR(
node_logging_interface_->get_logger( ),
"[%s] [ActionServer] %s", action_name_.c_str( ), msg.c_str( ) );
}
/**
* @brief Warn logging
*/
void warn_msg( const std::string & msg ) const
{
RCLCPP_WARN(
node_logging_interface_->get_logger( ),
"[%s] [ActionServer] %s", action_name_.c_str( ), msg.c_str( ) );
}
};
} // namespace nav2_util
#endif // NAV2_UTIL__SIMPLE_ACTION_SERVER_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_UTIL__STRING_UTILS_HPP_
#define NAV2_UTIL__STRING_UTILS_HPP_
#include <string>
#include <vector>
namespace nav2_util
{
typedef std::vector<std::string> Tokens;
/*
* @brief Remove leading slash from a topic name
* @param in String of topic in
* @return String out without slash
*/
31 std::string strip_leading_slash( const std::string & in );
///
/*
* @brief Split a string at the delimiters
* @param in String to split
* @param Delimiter criteria
* @return Tokens
*/
40 Tokens split( const std::string & tokenstring, char delimiter );
} // namespace nav2_util
#endif // NAV2_UTIL__STRING_UTILS_HPP_
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <vector>
#include <algorithm>
#include "nav2_util/costmap.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "nav2_util/geometry_utils.hpp"
using std::vector;
namespace nav2_util
{
using nav2_util::geometry_utils::orientationAroundZAxis;
const Costmap::CostValue Costmap::no_information = 255;
const Costmap::CostValue Costmap::lethal_obstacle = 254;
const Costmap::CostValue Costmap::inscribed_inflated_obstacle = 253;
const Costmap::CostValue Costmap::medium_cost = 128;
const Costmap::CostValue Costmap::free_space = 0;
// TODO( orduno ): Port ROS1 Costmap package
34 Costmap::Costmap(
35 rclcpp::Node * node, bool trinary_costmap, bool track_unknown_space,
int lethal_threshold, int unknown_cost_value )
: node_( node ), trinary_costmap_( trinary_costmap ), track_unknown_space_( track_unknown_space ),
lethal_threshold_( lethal_threshold ), unknown_cost_value_( unknown_cost_value )
{
if ( lethal_threshold_ < 0. || lethal_threshold_ > 100. ) {
RCLCPP_WARN(
node_->get_logger( ), "Costmap: Lethal threshold set to %d, it should be within"
" bounds 0-100. This could result in potential collisions!", lethal_threshold_ );
// lethal_threshold_ = std::max( std::min( lethal_threshold_, 100 ), 0 );
}
}
48 Costmap::~Costmap( )
{
}
52 void Costmap::set_static_map( const nav_msgs::msg::OccupancyGrid & occupancy_grid )
{
RCLCPP_INFO( node_->get_logger( ), "Costmap: Setting static costmap" );
costmap_properties_.map_load_time = node_->now( );
costmap_properties_.update_time = node_->now( );
costmap_properties_.layer = "Master";
// Store the properties of the occupancy grid
costmap_properties_.resolution = occupancy_grid.info.resolution;
costmap_properties_.size_x = occupancy_grid.info.width;
costmap_properties_.size_y = occupancy_grid.info.height;
costmap_properties_.origin = occupancy_grid.info.origin;
uint32_t size_x = costmap_properties_.size_x;
uint32_t size_y = costmap_properties_.size_y;
costs_.resize( size_x * size_y );
// TODO( orduno ): for now just doing a direct mapping of values from the original static map
// i.e. no cell inflation, etc.
std::vector<int8_t> static_map_cell_values = occupancy_grid.data;
unsigned int index = 0;
for ( unsigned int i = 0; i < size_y; ++i ) {
for ( unsigned int j = 0; j < size_x; ++j ) {
unsigned char value = static_map_cell_values[index];
costs_[index] = interpret_value( value );
++index;
}
}
map_provided_ = true;
}
87 void Costmap::set_test_costmap( const TestCostmap & testCostmapType )
{
costmap_properties_.map_load_time = node_->now( );
costmap_properties_.update_time = node_->now( );
costmap_properties_.layer = "master";
costmap_properties_.resolution = 1;
costmap_properties_.size_x = 10;
costmap_properties_.size_y = 10;
costmap_properties_.origin.position.x = 0.0;
costmap_properties_.origin.position.y = 0.0;
costmap_properties_.origin.position.z = 0.0;
// Define map rotation
// Provided as yaw with counterclockwise rotation, with yaw = 0 meaning no rotation
costmap_properties_.origin.orientation = orientationAroundZAxis( 0.0 );
costs_ = get_test_data( testCostmapType );
using_test_map_ = true;
}
108 nav2_msgs::msg::Costmap Costmap::get_costmap(
109 const nav2_msgs::msg::CostmapMetaData & /*specifications*/ )
{
if ( !map_provided_ && !using_test_map_ ) {
throw std::runtime_error( "Costmap has not been set." );
}
// TODO( orduno ): build a costmap given the specifications
// for now using the specs of the static map
nav2_msgs::msg::Costmap costmap;
costmap.header.stamp = node_->now( );
costmap.header.frame_id = "map";
costmap.metadata = costmap_properties_;
costmap.data = costs_;
return costmap;
}
128 vector<uint8_t> Costmap::get_test_data( const TestCostmap testCostmapType )
{
// TODO( orduno ): alternatively use a mathematical function
const uint8_t n = no_information;
const uint8_t x = lethal_obstacle;
const uint8_t i = inscribed_inflated_obstacle;
const uint8_t u = medium_cost;
const uint8_t o = free_space;
vector<uint8_t> costmapFree =
// 0 1 2 3 4 5 6 7 8 9
{o, o, o, o, o, o, o, o, o, o, // 0
o, o, o, o, o, o, o, o, o, o, // 1
o, o, o, o, o, o, o, o, o, o, // 2
o, o, o, o, o, o, o, o, o, o, // 3
o, o, o, o, o, o, o, o, o, o, // 4
o, o, o, o, o, o, o, o, o, o, // 5
o, o, o, o, o, o, o, o, o, o, // 6
o, o, o, o, o, o, o, o, o, o, // 7
o, o, o, o, o, o, o, o, o, o, // 8
o, o, o, o, o, o, o, o, o, o}; // 9
vector<uint8_t> costmapBounded =
// 0 1 2 3 4 5 6 7 8 9
{n, n, n, n, n, n, n, n, n, n, // 0
n, o, o, o, o, o, o, o, o, n, // 1
n, o, o, o, o, o, o, o, o, n, // 2
n, o, o, o, o, o, o, o, o, n, // 3
n, o, o, o, o, o, o, o, o, n, // 4
n, o, o, o, o, o, o, o, o, n, // 5
n, o, o, o, o, o, o, o, o, n, // 6
n, o, o, o, o, o, o, o, o, n, // 7
n, o, o, o, o, o, o, o, o, n, // 8
n, n, n, n, n, n, n, n, n, n}; // 9
vector<uint8_t> costmapObstacleBL =
// 0 1 2 3 4 5 6 7 8 9
{n, n, n, n, n, n, n, n, n, n, // 0
n, o, o, o, o, o, o, o, o, n, // 1
n, o, o, o, o, o, o, o, o, n, // 2
n, o, o, o, o, o, o, o, o, n, // 3
n, o, o, o, o, o, o, o, o, n, // 4
n, o, x, x, x, o, o, o, o, n, // 5
n, o, x, x, x, o, o, o, o, n, // 6
n, o, x, x, x, o, o, o, o, n, // 7
n, o, o, o, o, o, o, o, o, n, // 8
n, n, n, n, n, n, n, n, n, n}; // 9
vector<uint8_t> costmapObstacleTL =
// 0 1 2 3 4 5 6 7 8 9
{n, n, n, n, n, n, n, n, n, n, // 0
n, o, o, o, o, o, o, o, o, n, // 1
n, o, x, x, x, o, o, o, o, n, // 2
n, o, x, x, x, o, o, o, o, n, // 3
n, o, x, x, x, o, o, o, o, n, // 4
n, o, o, o, o, o, o, o, o, n, // 5
n, o, o, o, o, o, o, o, o, n, // 6
n, o, o, o, o, o, o, o, o, n, // 7
n, o, o, o, o, o, o, o, o, n, // 8
n, n, n, n, n, n, n, n, n, n}; // 9
vector<uint8_t> costmapMaze =
// 0 1 2 3 4 5 6 7 8 9
{n, n, n, n, n, n, n, n, n, n, // 0
n, o, o, o, o, o, o, o, o, n, // 1
n, x, x, o, x, x, x, o, x, n, // 2
n, o, o, o, o, x, o, o, o, n, // 3
n, o, x, x, o, x, o, x, o, n, // 4
n, o, x, x, o, x, o, x, o, n, // 5
n, o, o, x, o, x, o, x, o, n, // 6
n, x, o, x, o, x, o, x, o, n, // 7
n, o, o, o, o, o, o, x, o, n, // 8
n, n, n, n, n, n, n, n, n, n}; // 9
vector<uint8_t> costmapMaze2 =
// 0 1 2 3 4 5 6 7 8 9
{n, n, n, n, n, n, n, n, n, n, // 0
n, o, o, o, o, o, o, o, o, n, // 1
n, x, x, u, x, x, x, o, x, n, // 2
n, o, o, o, o, o, o, o, u, n, // 3
n, o, x, x, o, x, x, x, u, n, // 4
n, o, x, x, o, o, o, x, u, n, // 5
n, o, o, x, u, x, o, x, u, n, // 6
n, x, o, x, u, x, i, x, u, n, // 7
n, o, o, o, o, o, o, o, o, n, // 8
n, n, n, n, n, n, n, n, n, n}; // 9
switch ( testCostmapType ) {
case TestCostmap::open_space:
return costmapFree;
case TestCostmap::bounded:
return costmapBounded;
case TestCostmap::bottom_left_obstacle:
return costmapObstacleBL;
case TestCostmap::top_left_obstacle:
return costmapObstacleTL;
case TestCostmap::maze1:
return costmapMaze;
case TestCostmap::maze2:
return costmapMaze2;
default:
return costmapFree;
}
}
234 uint8_t Costmap::interpret_value( const int8_t value ) const
{
if ( track_unknown_space_ && value == unknown_cost_value_ ) {
return no_information;
} else if ( !track_unknown_space_ && value == unknown_cost_value_ ) {
return free_space;
} else if ( value >= lethal_threshold_ ) {
return lethal_obstacle;
} else if ( trinary_costmap_ ) {
return free_space;
}
double scale = static_cast<double>( value / lethal_threshold_ );
return static_cast<uint8_t>( scale * lethal_obstacle );
}
250 bool Costmap::is_free( const unsigned int x_coordinate, const unsigned int y_coordinate ) const
{
unsigned int index = y_coordinate * costmap_properties_.size_x + x_coordinate;
return is_free( index );
}
257 bool Costmap::is_free( const unsigned int index ) const
{
if ( costs_[index] < Costmap::inscribed_inflated_obstacle ) {
return true;
}
return false;
}
} // namespace nav2_util
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <vector>
#include <string>
#include <iostream>
#include <cstdlib>
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include "nav2_util/lifecycle_utils.hpp"
using std::cerr;
using namespace std::chrono_literals;
26 void usage( )
{
cerr << "Invalid command line.\n\n";
cerr << "This command will take a set of unconfigured lifecycle nodes through the\n";
cerr << "CONFIGURED to the ACTIVATED state\n";
cerr << "The nodes are brought up in the order listed on the command line\n\n";
cerr << "Usage:\n";
cerr << " > lifecycle_startup <node name> ...\n";
std::exit( 1 );
}
37 int main( int argc, char * argv[] )
{
if ( argc == 1 ) {
usage( );
}
rclcpp::init( 0, nullptr );
nav2_util::startup_lifecycle_nodes(
std::vector<std::string>( argv + 1, argv + argc ),
10s );
rclcpp::shutdown( );
}
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "nav2_util/lifecycle_node.hpp"
#include <memory>
#include <string>
#include <vector>
#include "lifecycle_msgs/msg/state.hpp"
namespace nav2_util
{
26 LifecycleNode::LifecycleNode(
27 const std::string & node_name,
28 const std::string & ns,
29 const rclcpp::NodeOptions & options )
: rclcpp_lifecycle::LifecycleNode( node_name, ns, options )
{
// server side never times out from lifecycle manager
this->declare_parameter( bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, true );
this->set_parameter(
rclcpp::Parameter(
bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, true ) );
printLifecycleNodeNotification( );
}
41 LifecycleNode::~LifecycleNode( )
{
RCLCPP_INFO( get_logger( ), "Destroying" );
// In case this lifecycle node wasn't properly shut down, do it here
if ( get_current_state( ).id( ) ==
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
{
on_deactivate( get_current_state( ) );
on_cleanup( get_current_state( ) );
}
}
53 void LifecycleNode::createBond( )
{
RCLCPP_INFO( get_logger( ), "Creating bond ( %s ) to lifecycle manager.", this->get_name( ) );
bond_ = std::make_unique<bond::Bond>(
std::string( "bond" ),
this->get_name( ),
shared_from_this( ) );
bond_->setHeartbeatPeriod( 0.10 );
bond_->setHeartbeatTimeout( 4.0 );
bond_->start( );
}
67 void LifecycleNode::destroyBond( )
{
RCLCPP_INFO( get_logger( ), "Destroying bond ( %s ) to lifecycle manager.", this->get_name( ) );
if ( bond_ ) {
bond_.reset( );
}
}
76 void LifecycleNode::printLifecycleNodeNotification( )
{
RCLCPP_INFO(
get_logger( ),
"\n\t%s lifecycle node launched. \n"
"\tWaiting on external lifecycle transitions to activate\n"
"\tSee https://design.ros2.org/articles/node_lifecycle.html for more information.", get_name( ) );
}
} // namespace nav2_util
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "nav2_util/lifecycle_service_client.hpp"
#include <string>
#include <chrono>
#include <memory>
#include "lifecycle_msgs/srv/change_state.hpp"
#include "lifecycle_msgs/srv/get_state.hpp"
using nav2_util::generate_internal_node;
using std::chrono::seconds;
using std::make_shared;
using std::string;
using namespace std::chrono_literals;
namespace nav2_util
{
33 LifecycleServiceClient::LifecycleServiceClient( const string & lifecycle_node_name )
: node_( generate_internal_node( lifecycle_node_name + "_lifecycle_client" ) ),
change_state_( lifecycle_node_name + "/change_state", node_ ),
get_state_( lifecycle_node_name + "/get_state", node_ )
{
// Block until server is up
get_state_.wait_for_service( );
}
42 LifecycleServiceClient::LifecycleServiceClient(
43 const string & lifecycle_node_name,
44 rclcpp::Node::SharedPtr parent_node )
: node_( parent_node ),
change_state_( lifecycle_node_name + "/change_state", node_ ),
get_state_( lifecycle_node_name + "/get_state", node_ )
{
// Block until server is up
get_state_.wait_for_service( );
}
53 bool LifecycleServiceClient::change_state(
54 const uint8_t transition,
55 const seconds timeout )
{
if ( !change_state_.wait_for_service( timeout ) ) {
throw std::runtime_error( "change_state service is not available!" );
}
auto request = std::make_shared<lifecycle_msgs::srv::ChangeState::Request>( );
request->transition.id = transition;
auto response = change_state_.invoke( request, timeout );
return response.get( );
}
67 bool LifecycleServiceClient::change_state(
68 std::uint8_t transition )
{
if ( !change_state_.wait_for_service( 5s ) ) {
throw std::runtime_error( "change_state service is not available!" );
}
auto request = std::make_shared<lifecycle_msgs::srv::ChangeState::Request>( );
auto response = std::make_shared<lifecycle_msgs::srv::ChangeState::Response>( );
request->transition.id = transition;
return change_state_.invoke( request, response );
}
80 uint8_t LifecycleServiceClient::get_state(
81 const seconds timeout )
{
if ( !get_state_.wait_for_service( timeout ) ) {
throw std::runtime_error( "get_state service is not available!" );
}
auto request = std::make_shared<lifecycle_msgs::srv::GetState::Request>( );
auto result = get_state_.invoke( request, timeout );
return result->current_state.id;
}
} // namespace nav2_util
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <chrono>
#include <string>
#include <thread>
#include <vector>
#include "lifecycle_msgs/srv/change_state.hpp"
#include "lifecycle_msgs/srv/get_state.hpp"
#include "nav2_util/lifecycle_service_client.hpp"
using std::string;
using lifecycle_msgs::msg::Transition;
namespace nav2_util
{
#define RETRY( fn, retries ) \
{ \
32 int count = 0; \
while ( true ) { \
try { \
fn; \
break; \
} catch ( std::runtime_error & e ) { \
++count; \
if ( count > ( retries ) ) { \
throw e;} \
} \
} \
}
static void startupLifecycleNode(
const std::string & node_name,
const std::chrono::seconds service_call_timeout,
const int retries )
{
50 LifecycleServiceClient sc( node_name );
// Despite waiting for the service to be available and using reliable transport
// service calls still frequently hang. To get reliable startup it's necessary
// to timeout the service call and retry it when that happens.
RETRY(
sc.change_state( Transition::TRANSITION_CONFIGURE, service_call_timeout ),
retries );
RETRY(
sc.change_state( Transition::TRANSITION_ACTIVATE, service_call_timeout ),
60 retries );
}
void startup_lifecycle_nodes(
const std::vector<std::string> & node_names,
const std::chrono::seconds service_call_timeout,
const int retries )
{
for ( const auto & node_name : node_names ) {
startupLifecycleNode( node_name, service_call_timeout, retries );
}
}
static void resetLifecycleNode(
const std::string & node_name,
const std::chrono::seconds service_call_timeout,
const int retries )
{
78 LifecycleServiceClient sc( node_name );
// Despite waiting for the service to be available and using reliable transport
// service calls still frequently hang. To get reliable reset it's necessary
// to timeout the service call and retry it when that happens.
RETRY(
sc.change_state( Transition::TRANSITION_DEACTIVATE, service_call_timeout ),
retries );
RETRY(
sc.change_state( Transition::TRANSITION_CLEANUP, service_call_timeout ),
retries );
}
void reset_lifecycle_nodes(
const std::vector<std::string> & node_names,
const std::chrono::seconds service_call_timeout,
const int retries )
{
for ( const auto & node_name : node_names ) {
resetLifecycleNode( node_name, service_call_timeout, retries );
}
}
} // namespace nav2_util
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include "nav2_util/node_thread.hpp"
namespace nav2_util
{
22 NodeThread::NodeThread( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base )
: node_( node_base )
{
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>( );
thread_ = std::make_unique<std::thread>(
[&]( )
{
executor_->add_node( node_ );
executor_->spin( );
executor_->remove_node( node_ );
} );
}
35 NodeThread::NodeThread( rclcpp::executors::SingleThreadedExecutor::SharedPtr executor )
: executor_( executor )
{
thread_ = std::make_unique<std::thread>( [&]( ) {executor_->spin( );} );
}
41 NodeThread::~NodeThread( )
{
executor_->cancel( );
thread_->join( );
}
} // namespace nav2_util
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "nav2_util/node_utils.hpp"
#include <chrono>
#include <string>
#include <algorithm>
#include <cctype>
using std::chrono::high_resolution_clock;
using std::to_string;
using std::string;
using std::replace_if;
using std::isalnum;
namespace nav2_util
{
30 string sanitize_node_name( const string & potential_node_name )
{
string node_name( potential_node_name );
// read this as `replace` characters in `node_name` `if` not alphanumeric.
// replace with '_'
replace_if(
begin( node_name ), end( node_name ),
[]( auto c ) {return !isalnum( c );},
'_' );
return node_name;
}
42 string add_namespaces( const string & top_ns, const string & sub_ns )
{
if ( !top_ns.empty( ) && top_ns.back( ) == '/' ) {
if ( top_ns.front( ) == '/' ) {
return top_ns + sub_ns;
} else {
return "/" + top_ns + sub_ns;
}
}
return top_ns + "/" + sub_ns;
}
55 std::string time_to_string( size_t len )
{
string output( len, '0' ); // prefill the string with zeros
auto timepoint = high_resolution_clock::now( );
auto timecount = timepoint.time_since_epoch( ).count( );
auto timestring = to_string( timecount );
if ( timestring.length( ) >= len ) {
// if `timestring` is shorter, put it at the end of `output`
output.replace(
0, len,
timestring,
timestring.length( ) - len, len );
} else {
// if `output` is shorter, just copy in the end of `timestring`
output.replace(
len - timestring.length( ), timestring.length( ),
timestring,
0, timestring.length( ) );
}
return output;
}
77 std::string generate_internal_node_name( const std::string & prefix )
{
return sanitize_node_name( prefix ) + "_" + time_to_string( 8 );
}
82 rclcpp::Node::SharedPtr generate_internal_node( const std::string & prefix )
{
auto options =
rclcpp::NodeOptions( )
.start_parameter_services( false )
.start_parameter_event_publisher( false )
.arguments( {"--ros-args", "-r", "__node:=" + generate_internal_node_name( prefix ), "--"} );
return rclcpp::Node::make_shared( "_", options );
}
rclcpp::NodeOptions
93 get_node_options_default( bool allow_undeclared, bool declare_initial_params )
{
rclcpp::NodeOptions options;
options.allow_undeclared_parameters( allow_undeclared );
options.automatically_declare_parameters_from_overrides( declare_initial_params );
return options;
}
} // namespace nav2_util
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include "nav2_util/odometry_utils.hpp"
using namespace std::chrono; // NOLINT
using namespace std::chrono_literals; // NOLINT
namespace nav2_util
{
26 OdomSmoother::OdomSmoother(
27 const rclcpp::Node::WeakPtr & parent,
double filter_duration,
29 const std::string & odom_topic )
: odom_history_duration_( rclcpp::Duration::from_seconds( filter_duration ) )
{
auto node = parent.lock( );
odom_sub_ = node->create_subscription<nav_msgs::msg::Odometry>(
odom_topic,
rclcpp::SystemDefaultsQoS( ),
std::bind( &OdomSmoother::odomCallback, this, std::placeholders::_1 ) );
odom_cumulate_.twist.twist.linear.x = 0;
odom_cumulate_.twist.twist.linear.y = 0;
odom_cumulate_.twist.twist.linear.z = 0;
odom_cumulate_.twist.twist.angular.x = 0;
odom_cumulate_.twist.twist.angular.y = 0;
odom_cumulate_.twist.twist.angular.z = 0;
}
46 OdomSmoother::OdomSmoother(
47 const nav2_util::LifecycleNode::WeakPtr & parent,
double filter_duration,
49 const std::string & odom_topic )
: odom_history_duration_( rclcpp::Duration::from_seconds( filter_duration ) )
{
auto node = parent.lock( );
odom_sub_ = node->create_subscription<nav_msgs::msg::Odometry>(
odom_topic,
rclcpp::SystemDefaultsQoS( ),
std::bind( &OdomSmoother::odomCallback, this, std::placeholders::_1 ) );
odom_cumulate_.twist.twist.linear.x = 0;
odom_cumulate_.twist.twist.linear.y = 0;
odom_cumulate_.twist.twist.linear.z = 0;
odom_cumulate_.twist.twist.angular.x = 0;
odom_cumulate_.twist.twist.angular.y = 0;
odom_cumulate_.twist.twist.angular.z = 0;
}
66 void OdomSmoother::odomCallback( const nav_msgs::msg::Odometry::SharedPtr msg )
{
std::lock_guard<std::mutex> lock( odom_mutex_ );
// update cumulated odom only if history is not empty
if ( !odom_history_.empty( ) ) {
// to store current time
auto current_time = rclcpp::Time( msg->header.stamp );
// to store time of the first odom in history
auto front_time = rclcpp::Time( odom_history_.front( ).header.stamp );
// update cumulated odom when duration has exceeded and pop earliest msg
while ( current_time - front_time > odom_history_duration_ ) {
const auto & odom = odom_history_.front( );
odom_cumulate_.twist.twist.linear.x -= odom.twist.twist.linear.x;
odom_cumulate_.twist.twist.linear.y -= odom.twist.twist.linear.y;
odom_cumulate_.twist.twist.linear.z -= odom.twist.twist.linear.z;
odom_cumulate_.twist.twist.angular.x -= odom.twist.twist.angular.x;
odom_cumulate_.twist.twist.angular.y -= odom.twist.twist.angular.y;
odom_cumulate_.twist.twist.angular.z -= odom.twist.twist.angular.z;
odom_history_.pop_front( );
if ( odom_history_.empty( ) ) {
break;
}
// update with the timestamp of earliest odom message in history
front_time = rclcpp::Time( odom_history_.front( ).header.stamp );
}
}
odom_history_.push_back( *msg );
updateState( );
}
102 void OdomSmoother::updateState( )
{
const auto & odom = odom_history_.back( );
odom_cumulate_.twist.twist.linear.x += odom.twist.twist.linear.x;
odom_cumulate_.twist.twist.linear.y += odom.twist.twist.linear.y;
odom_cumulate_.twist.twist.linear.z += odom.twist.twist.linear.z;
odom_cumulate_.twist.twist.angular.x += odom.twist.twist.angular.x;
odom_cumulate_.twist.twist.angular.y += odom.twist.twist.angular.y;
odom_cumulate_.twist.twist.angular.z += odom.twist.twist.angular.z;
vel_smooth_.header = odom.header;
vel_smooth_.twist.linear.x = odom_cumulate_.twist.twist.linear.x / odom_history_.size( );
vel_smooth_.twist.linear.y = odom_cumulate_.twist.twist.linear.y / odom_history_.size( );
vel_smooth_.twist.linear.z = odom_cumulate_.twist.twist.linear.z / odom_history_.size( );
vel_smooth_.twist.angular.x = odom_cumulate_.twist.twist.angular.x / odom_history_.size( );
vel_smooth_.twist.angular.y = odom_cumulate_.twist.twist.angular.y / odom_history_.size( );
vel_smooth_.twist.angular.z = odom_cumulate_.twist.twist.angular.z / odom_history_.size( );
}
} // namespace nav2_util
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2019 Steven Macenski
// Copyright ( c ) 2019 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include <memory>
#include "nav2_util/robot_utils.hpp"
#include "rclcpp/logger.hpp"
namespace nav2_util
{
26 bool getCurrentPose(
geometry_msgs::msg::PoseStamped & global_pose,
tf2_ros::Buffer & tf_buffer, const std::string global_frame,
const std::string robot_frame, const double transform_timeout,
const rclcpp::Time stamp )
{
tf2::toMsg( tf2::Transform::getIdentity( ), global_pose.pose );
global_pose.header.frame_id = robot_frame;
global_pose.header.stamp = stamp;
return transformPoseInTargetFrame(
global_pose, global_pose, tf_buffer, global_frame, transform_timeout );
}
40 bool transformPoseInTargetFrame(
const geometry_msgs::msg::PoseStamped & input_pose,
geometry_msgs::msg::PoseStamped & transformed_pose,
tf2_ros::Buffer & tf_buffer, const std::string target_frame,
const double transform_timeout )
{
static rclcpp::Logger logger = rclcpp::get_logger( "transformPoseInTargetFrame" );
try {
transformed_pose = tf_buffer.transform(
input_pose, target_frame,
tf2::durationFromSec( transform_timeout ) );
return true;
} catch ( tf2::LookupException & ex ) {
RCLCPP_ERROR(
logger,
"No Transform available Error looking up target frame: %s\n", ex.what( ) );
} catch ( tf2::ConnectivityException & ex ) {
RCLCPP_ERROR(
logger,
"Connectivity Error looking up target frame: %s\n", ex.what( ) );
} catch ( tf2::ExtrapolationException & ex ) {
RCLCPP_ERROR(
logger,
"Extrapolation Error looking up target frame: %s\n", ex.what( ) );
} catch ( tf2::TimeoutException & ex ) {
RCLCPP_ERROR(
logger,
"Transform timeout with tolerance: %.4f", transform_timeout );
} catch ( tf2::TransformException & ex ) {
RCLCPP_ERROR(
logger, "Failed to transform from %s to %s",
input_pose.header.frame_id.c_str( ), target_frame.c_str( ) );
}
return false;
}
} // end namespace nav2_util
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "nav2_util/string_utils.hpp"
#include <string>
using std::string;
namespace nav2_util
{
23 std::string strip_leading_slash( const string & in )
{
string out = in;
if ( ( !in.empty( ) ) && ( in[0] == '/' ) ) {
out.erase( 0, 1 );
}
return out;
}
34 Tokens split( const string & tokenstring, char delimiter )
{
Tokens tokens;
size_t current_pos = 0;
size_t pos = 0;
while ( ( pos = tokenstring.find( delimiter, current_pos ) ) != string::npos ) {
tokens.push_back( tokenstring.substr( current_pos, pos - current_pos ) );
current_pos = pos + 1;
}
tokens.push_back( tokenstring.substr( current_pos ) );
return tokens;
}
} // namespace nav2_util
// Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <chrono>
#include <memory>
#include <thread>
#include "gtest/gtest.h"
#include "nav2_util/node_utils.hpp"
#include "nav2_util/simple_action_server.hpp"
#include "test_msgs/action/fibonacci.hpp"
#include "std_msgs/msg/empty.hpp"
using Fibonacci = test_msgs::action::Fibonacci;
using GoalHandle = rclcpp_action::ServerGoalHandle<Fibonacci>;
using std::placeholders::_1;
using namespace std::chrono_literals;
31 class FibonacciServerNode : public rclcpp::Node
{
public:
34 FibonacciServerNode( )
: rclcpp::Node( "fibonacci_server_node" )
{
}
39 ~FibonacciServerNode( )
{
}
43 void on_init( )
{
action_server_ = std::make_shared<nav2_util::SimpleActionServer<Fibonacci>>(
shared_from_this( ),
"fibonacci",
std::bind( &FibonacciServerNode::execute, this ) );
deactivate_subs_ = create_subscription<std_msgs::msg::Empty>(
"deactivate_server",
1,
[this]( std_msgs::msg::Empty::UniquePtr /*msg*/ ) {
RCLCPP_INFO( this->get_logger( ), "Deactivating" );
action_server_->deactivate( );
} );
activate_subs_ = create_subscription<std_msgs::msg::Empty>(
"activate_server",
1,
[this]( std_msgs::msg::Empty::UniquePtr /*msg*/ ) {
RCLCPP_INFO( this->get_logger( ), "Activating" );
action_server_->activate( );
} );
omit_preempt_subs_ = create_subscription<std_msgs::msg::Empty>(
"omit_preemption",
1,
[this]( std_msgs::msg::Empty::UniquePtr /*msg*/ ) {
RCLCPP_INFO( this->get_logger( ), "Ignoring preemptions" );
do_premptions_ = false;
} );
}
75 void on_term( )
{
// when nothing's running make sure everything's dead.
const std::shared_ptr<const Fibonacci::Goal> a = action_server_->accept_pending_goal( );
const std::shared_ptr<const Fibonacci::Goal> b = action_server_->get_current_goal( );
assert( a == b );
assert( action_server_->is_cancel_requested( ) == false );
auto feedback = std::make_shared<Fibonacci::Feedback>( );
action_server_->publish_feedback( feedback );
action_server_.reset( );
}
87 void execute( )
{
rclcpp::Rate loop_rate( 10 );
preempted:
// Initialize the goal, feedback, and result
auto goal = action_server_->get_current_goal( );
auto feedback = std::make_shared<Fibonacci::Feedback>( );
auto result = std::make_shared<Fibonacci::Result>( );
// Fibonacci-specific initialization
auto & sequence = feedback->sequence;
sequence.push_back( 0 );
sequence.push_back( 1 );
for ( int i = 1; ( i < goal->order ) && rclcpp::ok( ); ++i ) {
// Should be check periodically if this action has been canceled
// or if the server has been deactivated.
if ( action_server_->is_cancel_requested( ) || !action_server_->is_server_active( ) ) {
result->sequence = sequence;
return;
}
// Check if we've gotten an new goal, pre-empting the current one
if ( do_premptions_ && action_server_->is_preempt_requested( ) ) {
action_server_->accept_pending_goal( );
goto preempted;
}
// Update the sequence
sequence.push_back( sequence[i] + sequence[i - 1] );
// Publish feedback
action_server_->publish_feedback( feedback );
loop_rate.sleep( );
}
// Check if goal is done
if ( rclcpp::ok( ) ) {
result->sequence = sequence;
action_server_->succeeded_current( result );
}
}
private:
std::shared_ptr<nav2_util::SimpleActionServer<Fibonacci>> action_server_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr deactivate_subs_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr activate_subs_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr omit_preempt_subs_;
bool do_premptions_{true};
};
class RclCppFixture
{
public:
RclCppFixture( )
{
}
void Setup( )
{
server_thread_ =
std::make_shared<std::thread>( std::bind( &RclCppFixture::server_thread_func, this ) );
}
~RclCppFixture( )
{
server_thread_->join( );
}
void server_thread_func( )
{
auto node = std::make_shared<FibonacciServerNode>( );
node->on_init( );
rclcpp::spin( node->get_node_base_interface( ) );
node->on_term( );
node.reset( );
}
std::shared_ptr<std::thread> server_thread_;
};
RclCppFixture g_rclcppfixture;
class ActionTestNode : public rclcpp::Node
{
public:
ActionTestNode( )
: rclcpp::Node( nav2_util::generate_internal_node_name( "action_test_node" ) )
{
}
void on_init( )
{
action_client_ = rclcpp_action::create_client<Fibonacci>( shared_from_this( ), "fibonacci" );
action_client_->wait_for_action_server( );
deactivate_pub_ = this->create_publisher<std_msgs::msg::Empty>( "deactivate_server", 1 );
activate_pub_ = this->create_publisher<std_msgs::msg::Empty>( "activate_server", 1 );
omit_prempt_pub_ = this->create_publisher<std_msgs::msg::Empty>( "omit_preemption", 1 );
}
void on_term( )
{
action_client_.reset( );
}
void deactivate_server( )
{
deactivate_pub_->publish( std_msgs::msg::Empty( ) );
}
void activate_server( )
{
activate_pub_->publish( std_msgs::msg::Empty( ) );
}
void omit_server_preemptions( )
{
omit_prempt_pub_->publish( std_msgs::msg::Empty( ) );
}
rclcpp_action::Client<Fibonacci>::SharedPtr action_client_;
rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr deactivate_pub_;
rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr activate_pub_;
rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr omit_prempt_pub_;
};
class ActionTest : public ::testing::Test
{
protected:
void SetUp( ) override
{
node_ = std::make_shared<ActionTestNode>( );
node_->on_init( );
}
void TearDown( ) override
{
std::cout << " Teardown" << std::endl;
node_->on_term( );
std::cout << " Teardown..." << std::endl;
node_.reset( );
std::cout << " Teardown complete" << std::endl;
}
std::shared_ptr<ActionTestNode> node_;
};
TEST_F( ActionTest, test_simple_action )
{
node_->activate_server( );
// The goal for this invocation
auto goal = Fibonacci::Goal( );
goal.order = 12;
// Send the goal
auto future_goal_handle = node_->action_client_->async_send_goal( goal );
EXPECT_EQ(
rclcpp::spin_until_future_complete(
node_,
future_goal_handle ), rclcpp::FutureReturnCode::SUCCESS );
auto goal_handle = future_goal_handle.get( );
// Wait for the result
auto future_result = node_->action_client_->async_get_result( goal_handle );
EXPECT_EQ(
rclcpp::spin_until_future_complete( node_, future_result ),
rclcpp::FutureReturnCode::SUCCESS );
// The final result
rclcpp_action::ClientGoalHandle<Fibonacci>::WrappedResult result = future_result.get( );
EXPECT_EQ( result.code, rclcpp_action::ResultCode::SUCCEEDED );
// Sum all of the values in the requested fibonacci series
int sum = 0;
for ( auto number : result.result->sequence ) {
sum += number;
}
EXPECT_EQ( sum, 376 );
SUCCEED( );
}
TEST_F( ActionTest, test_simple_action_with_feedback )
{
int feedback_sum = 0;
// A callback to accumulate the intermediate values
auto feedback_callback = [&feedback_sum](
rclcpp_action::ClientGoalHandle<Fibonacci>::SharedPtr /*goal_handle*/,
const std::shared_ptr<const Fibonacci::Feedback> feedback )
{
feedback_sum += feedback->sequence.back( );
};
// The goal for this invocation
auto goal = Fibonacci::Goal( );
goal.order = 10;
auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions( );
send_goal_options.feedback_callback = feedback_callback;
// Send the goal
auto future_goal_handle = node_->action_client_->async_send_goal( goal, send_goal_options );
EXPECT_EQ(
rclcpp::spin_until_future_complete(
node_,
future_goal_handle ), rclcpp::FutureReturnCode::SUCCESS );
auto goal_handle = future_goal_handle.get( );
// Wait for the result
auto future_result = node_->action_client_->async_get_result( goal_handle );
EXPECT_EQ(
rclcpp::spin_until_future_complete(
node_,
future_result ), rclcpp::FutureReturnCode::SUCCESS );
// The final result
rclcpp_action::ClientGoalHandle<Fibonacci>::WrappedResult result = future_result.get( );
EXPECT_EQ( result.code, rclcpp_action::ResultCode::SUCCEEDED );
// Sum all of the values in the requested fibonacci series
int sum = 0;
for ( auto number : result.result->sequence ) {
sum += number;
}
EXPECT_EQ( sum, 143 );
EXPECT_GE( feedback_sum, 0 ); // We should have received *some* feedback
SUCCEED( );
}
TEST_F( ActionTest, test_simple_action_activation_cycling )
{
// The goal for this invocation
auto goal = Fibonacci::Goal( );
// Sending a goal that will take a long time to calculate
goal.order = 12'000'000;
// Start by sending goal on an active server
// Send the goal
auto future_goal_handle = node_->action_client_->async_send_goal( goal );
EXPECT_EQ(
rclcpp::spin_until_future_complete(
node_,
future_goal_handle ), rclcpp::FutureReturnCode::SUCCESS );
// Deactivate while running
node_->deactivate_server( );
auto goal_handle = future_goal_handle.get( );
// Wait for the result
auto future_result = node_->action_client_->async_get_result( goal_handle );
EXPECT_EQ(
rclcpp::spin_until_future_complete( node_, future_result ),
rclcpp::FutureReturnCode::SUCCESS );
// The action should be reported as aborted.
EXPECT_EQ( future_result.get( ).code, rclcpp_action::ResultCode::ABORTED );
// Cycle back to active
node_->activate_server( );
goal.order = 12;
// Send the goal
future_goal_handle = node_->action_client_->async_send_goal( goal );
std::cout << "Sent goal, spinning til complete..." << std::endl;
EXPECT_EQ(
rclcpp::spin_until_future_complete(
node_,
future_goal_handle ), rclcpp::FutureReturnCode::SUCCESS );
goal_handle = future_goal_handle.get( );
// Wait for the result
future_result = node_->action_client_->async_get_result( goal_handle );
std::cout << "Getting result, spinning til complete..." << std::endl;
EXPECT_EQ(
rclcpp::spin_until_future_complete( node_, future_result ),
rclcpp::FutureReturnCode::SUCCESS );
// Now the action should have been successfully executed.
EXPECT_EQ( future_result.get( ).code, rclcpp_action::ResultCode::SUCCEEDED );
SUCCEED( );
}
TEST_F( ActionTest, test_simple_action_preemption )
{
// The goal for this invocation
auto goal = Fibonacci::Goal( );
// Sending a goal that will take a long time to calculate
goal.order = 12'000'000;
// Send the goal
auto future_goal_handle = node_->action_client_->async_send_goal( goal );
std::cout << "Sent goal, spinning til complete..." << std::endl;
EXPECT_EQ(
rclcpp::spin_until_future_complete(
node_,
future_goal_handle ), rclcpp::FutureReturnCode::SUCCESS );
// Preempt the goal
auto preemption_goal = Fibonacci::Goal( );
preemption_goal.order = 1;
// Send the goal
future_goal_handle = node_->action_client_->async_send_goal( preemption_goal );
std::cout << "Sent goal, spinning til complete..." << std::endl;
EXPECT_EQ(
rclcpp::spin_until_future_complete(
node_,
future_goal_handle ), rclcpp::FutureReturnCode::SUCCESS );
auto goal_handle = future_goal_handle.get( );
// Wait for the result
auto future_result = node_->action_client_->async_get_result( goal_handle );
std::cout << "Getting result, spinning til complete..." << std::endl;
EXPECT_EQ(
rclcpp::spin_until_future_complete( node_, future_result ),
rclcpp::FutureReturnCode::SUCCESS );
// The final result
rclcpp_action::ClientGoalHandle<Fibonacci>::WrappedResult result = future_result.get( );
EXPECT_EQ( result.code, rclcpp_action::ResultCode::SUCCEEDED );
// Sum all of the values in the requested fibonacci series
int sum = 0;
for ( auto number : result.result->sequence ) {
sum += number;
}
EXPECT_EQ( sum, 1 );
SUCCEED( );
}
TEST_F( ActionTest, test_simple_action_preemption_after_succeeded )
{
// Test race condition between successfully completing an action and receiving a preemption.
auto goal = Fibonacci::Goal( );
goal.order = 20;
auto preemption = Fibonacci::Goal( );
preemption.order = 1;
// Send the goal
auto future_goal_handle = node_->action_client_->async_send_goal( goal );
EXPECT_EQ(
rclcpp::spin_until_future_complete(
node_,
future_goal_handle ), rclcpp::FutureReturnCode::SUCCESS );
node_->omit_server_preemptions( );
auto future_preempt_handle = node_->action_client_->async_send_goal( preemption );
EXPECT_EQ(
rclcpp::spin_until_future_complete(
node_,
future_goal_handle ), rclcpp::FutureReturnCode::SUCCESS );
// Get the results
auto goal_handle = future_goal_handle.get( );
// Wait for the result of initial goal
auto future_result = node_->action_client_->async_get_result( goal_handle );
EXPECT_EQ(
rclcpp::spin_until_future_complete( node_, future_result ),
rclcpp::FutureReturnCode::SUCCESS );
// The final result
rclcpp_action::ClientGoalHandle<Fibonacci>::WrappedResult result = future_result.get( );
EXPECT_EQ( result.code, rclcpp_action::ResultCode::SUCCEEDED );
// Sum all of the values in the requested fibonacci series
int sum = 0;
for ( auto number : result.result->sequence ) {
sum += number;
}
EXPECT_EQ( sum, 17710 );
// Now get the preemption result
goal_handle = future_preempt_handle.get( );
// Wait for the result of initial goal
future_result = node_->action_client_->async_get_result( goal_handle );
ASSERT_EQ(
rclcpp::spin_until_future_complete( node_, future_result ),
rclcpp::FutureReturnCode::SUCCESS );
// The final result
result = future_result.get( );
EXPECT_EQ( result.code, rclcpp_action::ResultCode::SUCCEEDED );
// Sum all of the values in the requested fibonacci series
sum = 0;
for ( auto number : result.result->sequence ) {
sum += number;
}
EXPECT_EQ( sum, 1 );
SUCCEED( );
}
TEST_F( ActionTest, test_handle_goal_deactivated )
{
node_->deactivate_server( );
auto goal = Fibonacci::Goal( );
goal.order = 12;
// Send the goal
auto future_goal_handle = node_->action_client_->async_send_goal( goal );
EXPECT_EQ(
rclcpp::spin_until_future_complete(
node_,
future_goal_handle ), rclcpp::FutureReturnCode::SUCCESS );
node_->activate_server( );
SUCCEED( );
}
TEST_F( ActionTest, test_handle_cancel )
{
auto goal = Fibonacci::Goal( );
goal.order = 14000000;
// Send the goal
auto future_goal_handle = node_->action_client_->async_send_goal( goal );
EXPECT_EQ(
rclcpp::spin_until_future_complete(
node_,
future_goal_handle ), rclcpp::FutureReturnCode::SUCCESS );
// Cancel the goal
auto cancel_response = node_->action_client_->async_cancel_goal( future_goal_handle.get( ) );
EXPECT_EQ(
rclcpp::spin_until_future_complete(
node_,
cancel_response ), rclcpp::FutureReturnCode::SUCCESS );
// Check cancelled
EXPECT_EQ( future_goal_handle.get( )->get_status( ), rclcpp_action::GoalStatus::STATUS_CANCELING );
SUCCEED( );
}
int main( int argc, char ** argv )
{
rclcpp::init( argc, argv );
g_rclcppfixture.Setup( );
::testing::InitGoogleTest( &argc, argv );
auto result = RUN_ALL_TESTS( );
rclcpp::shutdown( );
rclcpp::Rate( 1 ).sleep( );
return result;
}
1 // Copyright ( c ) 2018 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <chrono>
#include <thread>
#include "nav2_util/execution_timer.hpp"
#include "gtest/gtest.h"
using nav2_util::ExecutionTimer;
using std::this_thread::sleep_for;
using namespace std::chrono_literals;
25 TEST( ExecutionTimer, BasicDelay )
{
ExecutionTimer t;
t.start( );
sleep_for( 10ns );
t.end( );
ASSERT_GE( t.elapsed_time( ), 10ns );
ASSERT_GE( t.elapsed_time_in_seconds( ), 1e-8 );
}
1 // Copyright ( c ) 2018 Intel Corporation
// Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "nav2_util/geometry_utils.hpp"
#include "geometry_msgs/msg/point.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "nav_msgs/msg/path.hpp"
#include "gtest/gtest.h"
using nav2_util::geometry_utils::euclidean_distance;
using nav2_util::geometry_utils::calculate_path_length;
25 TEST( GeometryUtils, euclidean_distance_point_3d )
{
geometry_msgs::msg::Point point1;
point1.x = 3.0;
point1.y = 2.0;
point1.z = 1.0;
geometry_msgs::msg::Point point2;
point2.x = 1.0;
point2.y = 2.0;
point2.z = 3.0;
ASSERT_NEAR( euclidean_distance( point1, point2, true ), 2.82843, 1e-5 );
}
40 TEST( GeometryUtils, euclidean_distance_point_2d )
{
geometry_msgs::msg::Point point1;
point1.x = 3.0;
point1.y = 2.0;
point1.z = 1.0;
geometry_msgs::msg::Point point2;
point2.x = 1.0;
point2.y = 2.0;
point2.z = 3.0;
ASSERT_NEAR( euclidean_distance( point1, point2 ), 2.0, 1e-5 );
}
55 TEST( GeometryUtils, euclidean_distance_pose_3d )
{
geometry_msgs::msg::Pose pose1;
pose1.position.x = 7.0;
pose1.position.y = 4.0;
pose1.position.z = 3.0;
geometry_msgs::msg::Pose pose2;
pose2.position.x = 17.0;
pose2.position.y = 6.0;
pose2.position.z = 2.0;
ASSERT_NEAR( euclidean_distance( pose1, pose2, true ), 10.24695, 1e-5 );
}
70 TEST( GeometryUtils, euclidean_distance_pose_2d )
{
geometry_msgs::msg::Pose pose1;
pose1.position.x = 7.0;
pose1.position.y = 4.0;
pose1.position.z = 3.0;
geometry_msgs::msg::Pose pose2;
pose2.position.x = 17.0;
pose2.position.y = 6.0;
pose2.position.z = 2.0;
ASSERT_NEAR( euclidean_distance( pose1, pose2 ), 10.19804, 1e-5 );
}
85 TEST( GeometryUtils, calculate_path_length )
{
nav_msgs::msg::Path straight_line_path;
size_t nb_path_points = 10;
float distance_between_poses = 2.0;
float current_x_loc = 0.0;
for ( size_t i = 0; i < nb_path_points; ++i ) {
geometry_msgs::msg::PoseStamped pose_stamped_msg;
pose_stamped_msg.pose.position.x = current_x_loc;
straight_line_path.poses.push_back( pose_stamped_msg );
current_x_loc += distance_between_poses;
}
ASSERT_NEAR(
calculate_path_length( straight_line_path ),
( nb_path_points - 1 ) * distance_between_poses, 1e-5 );
ASSERT_NEAR(
calculate_path_length( straight_line_path, straight_line_path.poses.size( ) ),
0.0, 1e-5 );
nav_msgs::msg::Path circle_path;
float polar_distance = 2.0;
uint32_t current_polar_angle_deg = 0;
constexpr float pi = 3.14159265358979;
while ( current_polar_angle_deg != 360 ) {
float x_loc = polar_distance * std::cos( current_polar_angle_deg * ( pi / 180.0 ) );
float y_loc = polar_distance * std::sin( current_polar_angle_deg * ( pi / 180.0 ) );
geometry_msgs::msg::PoseStamped pose_stamped_msg;
pose_stamped_msg.pose.position.x = x_loc;
pose_stamped_msg.pose.position.y = y_loc;
circle_path.poses.push_back( pose_stamped_msg );
current_polar_angle_deg += 1;
}
ASSERT_NEAR(
calculate_path_length( circle_path ),
2 * pi * polar_distance, 1e-1 );
}
1 // Copyright ( c ) 2020 Samsung Research
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_UTIL__TEST__TEST_LIFECYCLE_CLI_NODE_HPP_
#define NAV2_UTIL__TEST__TEST_LIFECYCLE_CLI_NODE_HPP_
#include <cstdlib>
#include <memory>
#include "gtest/gtest.h"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/lifecycle_utils.hpp"
#include "nav2_util/node_thread.hpp"
#include "rclcpp/rclcpp.hpp"
#ifdef _WIN32
#include <windows.h>
#endif
30 class DummyNode : public nav2_util::LifecycleNode
{
public:
33 DummyNode( )
: nav2_util::LifecycleNode( "nav2_test_cli", "" )
{
activated = false;
}
39 nav2_util::CallbackReturn on_activate( const rclcpp_lifecycle::State & /*state*/ )
{
activated = true;
return nav2_util::CallbackReturn::SUCCESS;
}
45 bool activated;
};
48 class Handle
{
public:
51 Handle( )
{
node = std::make_shared<DummyNode>( );
thread = std::make_shared<nav2_util::NodeThread>( node->get_node_base_interface( ) );
}
56 ~Handle( )
{
thread.reset( );
node.reset( );
}
62 std::shared_ptr<nav2_util::NodeThread> thread;
63 std::shared_ptr<DummyNode> node;
};
66 class RclCppFixture
{
public:
69 RclCppFixture( )
{
rclcpp::init( 0, nullptr );
}
74 ~RclCppFixture( )
{
rclcpp::shutdown( );
}
};
RclCppFixture g_rclcppfixture;
82 TEST( LifeycleCLI, fails_no_node_name )
{
Handle handle;
auto rc = system( "ros2 run nav2_util lifecycle_bringup" );
( void )rc;
#ifdef _WIN32
Sleep( 1000 );
#else
sleep( 1 );
#endif
// check node didn't mode
EXPECT_EQ( handle.node->activated, false );
SUCCEED( );
}
97 TEST( LifeycleCLI, succeeds_node_name )
{
Handle handle;
auto rc = system( "ros2 run nav2_util lifecycle_bringup nav2_test_cli" );
#ifdef _WIN32
Sleep( 3000 );
#else
sleep( 3 );
#endif
// check node moved
( void )rc;
EXPECT_EQ( handle.node->activated, true );
SUCCEED( );
}
#endif // NAV2_UTIL__TEST__TEST_LIFECYCLE_CLI_NODE_HPP_
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include "gtest/gtest.h"
#include "nav2_util/lifecycle_node.hpp"
#include "rclcpp/rclcpp.hpp"
21 class RclCppFixture
{
public:
24 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
25 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
// For the following two tests, if the LifecycleNode doesn't shut down properly,
// the overall test will hang since the rclcpp thread will still be running,
// preventing the executable from exiting ( the test will hang )
33 TEST( LifecycleNode, RclcppNodeExitsCleanly )
{
// Make sure the node exits cleanly when using an rclcpp_node and associated thread
auto node1 = std::make_shared<nav2_util::LifecycleNode>( "test_node", "" );
std::this_thread::sleep_for( std::chrono::seconds( 1 ) );
SUCCEED( );
}
41 TEST( LifecycleNode, MultipleRclcppNodesExitCleanly )
{
// Try a couple nodes w/ rclcpp_node and threads
auto node1 = std::make_shared<nav2_util::LifecycleNode>( "test_node1", "" );
auto node2 = std::make_shared<nav2_util::LifecycleNode>( "test_node2", "" );
std::this_thread::sleep_for( std::chrono::seconds( 1 ) );
SUCCEED( );
}
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <thread>
#include <vector>
#include "gtest/gtest.h"
#include "nav2_util/lifecycle_utils.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "rclcpp/rclcpp.hpp"
using nav2_util::startup_lifecycle_nodes;
using nav2_util::reset_lifecycle_nodes;
26 class RclCppFixture
{
public:
29 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
30 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
34 void SpinNodesUntilDone(
std::vector<rclcpp_lifecycle::LifecycleNode::SharedPtr> nodes,
std::atomic<bool> * test_done )
{
rclcpp::executors::SingleThreadedExecutor exec;
for ( const auto & node : nodes ) {
exec.add_node( node->get_node_base_interface( ) );
}
while ( rclcpp::ok( ) && !( *test_done ) ) {
exec.spin_some( );
}
}
47 TEST( Lifecycle, interface )
{
std::vector<rclcpp_lifecycle::LifecycleNode::SharedPtr> nodes;
nodes.push_back( rclcpp_lifecycle::LifecycleNode::make_shared( "foo" ) );
nodes.push_back( rclcpp_lifecycle::LifecycleNode::make_shared( "bar" ) );
std::atomic<bool> done( false );
std::thread node_thread( SpinNodesUntilDone, nodes, &done );
startup_lifecycle_nodes( "/foo:/bar" );
reset_lifecycle_nodes( "/foo:/bar" );
done = true;
node_thread.join( );
SUCCEED( );
}
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include <string>
#include "nav2_util/node_utils.hpp"
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
using nav2_util::sanitize_node_name;
using nav2_util::generate_internal_node_name;
using nav2_util::generate_internal_node;
using nav2_util::add_namespaces;
using nav2_util::time_to_string;
using nav2_util::declare_parameter_if_not_declared;
using nav2_util::get_plugin_type_param;
30 class RclCppFixture
{
public:
33 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
34 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
38 TEST( SanitizeNodeName, SanitizeNodeName )
{
ASSERT_EQ( sanitize_node_name( "bar" ), "bar" );
ASSERT_EQ( sanitize_node_name( "/foo/bar" ), "_foo_bar" );
}
44 TEST( TimeToString, IsLengthCorrect )
{
ASSERT_EQ( time_to_string( 0 ).length( ), 0u );
ASSERT_EQ( time_to_string( 1 ).length( ), 1u );
ASSERT_EQ( time_to_string( 10 ).length( ), 10u );
ASSERT_EQ( time_to_string( 20 )[0], '0' );
}
52 TEST( TimeToString, TimeToStringDifferent )
{
auto time1 = time_to_string( 8 );
auto time2 = time_to_string( 8 );
ASSERT_NE( time1, time2 );
}
59 TEST( GenerateInternalNodeName, GenerateNodeName )
{
auto defaultName = generate_internal_node_name( );
ASSERT_EQ( defaultName[0], '_' );
ASSERT_EQ( defaultName.length( ), 9u );
}
66 TEST( AddNamespaces, AddNamespaceSlash )
{
ASSERT_EQ( add_namespaces( "hi", "bye" ), "hi/bye" );
ASSERT_EQ( add_namespaces( "hi/", "bye" ), "/hi/bye" );
}
72 TEST( DeclareParameterIfNotDeclared, DeclareParameterIfNotDeclared )
{
auto node = std::make_shared<rclcpp::Node>( "test_node" );
std::string param;
// test declared parameter
node->declare_parameter( "foobar", "foo" );
declare_parameter_if_not_declared( node, "foobar", rclcpp::ParameterValue{"bar"} );
node->get_parameter( "foobar", param );
ASSERT_EQ( param, "foo" );
// test undeclared parameter
declare_parameter_if_not_declared( node, "waldo", rclcpp::ParameterValue{"fred"} );
node->get_parameter( "waldo", param );
ASSERT_EQ( param, "fred" );
}
89 TEST( GetPluginTypeParam, GetPluginTypeParam )
{
::testing::FLAGS_gtest_death_test_style = "threadsafe";
auto node = std::make_shared<rclcpp::Node>( "test_node" );
node->declare_parameter( "Foo.plugin", "bar" );
ASSERT_EQ( get_plugin_type_param( node, "Foo" ), "bar" );
ASSERT_EXIT( get_plugin_type_param( node, "Waldo" ), ::testing::ExitedWithCode( 255 ), ".*" );
}
1 // Copyright ( c ) 2020 Sarthak Mittal
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include "nav2_util/odometry_utils.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "gtest/gtest.h"
using namespace std::chrono; // NOLINT
using namespace std::chrono_literals; // NOLINT
27 class RclCppFixture
{
public:
30 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
31 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
35 TEST( OdometryUtils, test_smoothed_velocity )
{
auto node = std::make_shared<rclcpp::Node>( "test_node" );
auto odom_pub = node->create_publisher<nav_msgs::msg::Odometry>( "odom", 1 );
nav2_util::OdomSmoother odom_smoother( node, 0.3, "odom" );
nav_msgs::msg::Odometry odom_msg;
geometry_msgs::msg::Twist twist_msg;
auto time = node->now( );
odom_msg.header.stamp = time;
odom_msg.twist.twist.linear.x = 1.0;
odom_msg.twist.twist.linear.y = 1.0;
odom_msg.twist.twist.angular.z = 1.0;
odom_pub->publish( odom_msg );
rclcpp::spin_some( node );
twist_msg = odom_smoother.getTwist( );
EXPECT_EQ( twist_msg.linear.x, 1.0 );
EXPECT_EQ( twist_msg.linear.y, 1.0 );
EXPECT_EQ( twist_msg.angular.z, 1.0 );
odom_msg.header.stamp = time + rclcpp::Duration::from_seconds( 0.1 );
odom_msg.twist.twist.linear.x = 2.0;
odom_msg.twist.twist.linear.y = 2.0;
odom_msg.twist.twist.angular.z = 2.0;
odom_pub->publish( odom_msg );
std::this_thread::sleep_for( 100ms );
rclcpp::spin_some( node );
twist_msg = odom_smoother.getTwist( );
EXPECT_EQ( twist_msg.linear.x, 1.5 );
EXPECT_EQ( twist_msg.linear.y, 1.5 );
EXPECT_EQ( twist_msg.angular.z, 1.5 );
odom_msg.header.stamp = time + rclcpp::Duration::from_seconds( 0.2 );
odom_msg.twist.twist.linear.x = 3.0;
odom_msg.twist.twist.linear.y = 3.0;
odom_msg.twist.twist.angular.z = 3.0;
odom_pub->publish( odom_msg );
std::this_thread::sleep_for( 100ms );
rclcpp::spin_some( node );
twist_msg = odom_smoother.getTwist( );
EXPECT_EQ( twist_msg.linear.x, 2.0 );
EXPECT_EQ( twist_msg.linear.y, 2.0 );
EXPECT_EQ( twist_msg.angular.z, 2.0 );
odom_msg.header.stamp = time + rclcpp::Duration::from_seconds( 0.45 );
odom_msg.twist.twist.linear.x = 4.0;
odom_msg.twist.twist.linear.y = 4.0;
odom_msg.twist.twist.angular.z = 4.0;
odom_pub->publish( odom_msg );
std::this_thread::sleep_for( 100ms );
rclcpp::spin_some( node );
twist_msg = odom_smoother.getTwist( );
EXPECT_EQ( twist_msg.linear.x, 3.5 );
EXPECT_EQ( twist_msg.linear.y, 3.5 );
EXPECT_EQ( twist_msg.angular.z, 3.5 );
odom_msg.header.stamp = time + rclcpp::Duration::from_seconds( 1.0 );
odom_msg.twist.twist.linear.x = 5.0;
odom_msg.twist.twist.linear.y = 5.0;
odom_msg.twist.twist.angular.z = 5.0;
odom_pub->publish( odom_msg );
std::this_thread::sleep_for( 100ms );
rclcpp::spin_some( node );
twist_msg = odom_smoother.getTwist( );
EXPECT_EQ( twist_msg.linear.x, 5.0 );
EXPECT_EQ( twist_msg.linear.y, 5.0 );
EXPECT_EQ( twist_msg.angular.z, 5.0 );
}
1 // Copyright ( c ) 2020 Samsung Research
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "nav2_util/robot_utils.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "gtest/gtest.h"
#include "nav2_util/node_thread.hpp"
#include "tf2_ros/create_timer_ros.h"
25 TEST( RobotUtils, LookupExceptionError )
{
rclcpp::init( 0, nullptr );
auto node = std::make_shared<rclcpp::Node>( "name", rclcpp::NodeOptions( ) );
geometry_msgs::msg::PoseStamped global_pose;
tf2_ros::Buffer tf( node->get_clock( ) );
ASSERT_FALSE( nav2_util::getCurrentPose( global_pose, tf, "map", "base_link", 0.1 ) );
global_pose.header.frame_id = "base_link";
ASSERT_FALSE( nav2_util::transformPoseInTargetFrame( global_pose, global_pose, tf, "map", 0.1 ) );
}
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include <string>
#include "nav2_util/service_client.hpp"
#include "rclcpp/rclcpp.hpp"
#include "std_srvs/srv/empty.hpp"
#include "std_msgs/msg/empty.hpp"
#include "gtest/gtest.h"
using nav2_util::ServiceClient;
using std::string;
26 class RclCppFixture
{
public:
29 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
30 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
34 class TestServiceClient : public ServiceClient<std_srvs::srv::Empty>
{
public:
37 TestServiceClient(
const std::string & name,
const rclcpp::Node::SharedPtr & provided_node = rclcpp::Node::SharedPtr( ) )
: ServiceClient( name, provided_node ) {}
42 string name( ) {return node_->get_name( );}
43 const rclcpp::Node::SharedPtr & getNode( ) {return node_;}
};
46 TEST( ServiceClient, can_ServiceClient_use_passed_in_node )
{
auto node = rclcpp::Node::make_shared( "test_node" );
TestServiceClient t( "bar", node );
ASSERT_EQ( t.getNode( ), node );
ASSERT_EQ( t.name( ), "test_node" );
}
54 TEST( ServiceClient, can_ServiceClient_invoke_in_callback )
{
int a = 0;
auto service_node = rclcpp::Node::make_shared( "service_node" );
auto service = service_node->create_service<std_srvs::srv::Empty>(
"empty_srv",
[&a]( std_srvs::srv::Empty::Request::SharedPtr, std_srvs::srv::Empty::Response::SharedPtr ) {
a = 1;
} );
auto srv_thread = std::thread( [&]( ) {rclcpp::spin( service_node );} );
auto pub_node = rclcpp::Node::make_shared( "pub_node" );
auto pub = pub_node->create_publisher<std_msgs::msg::Empty>(
"empty_topic",
rclcpp::QoS( 1 ).transient_local( ) );
auto pub_thread = std::thread( [&]( ) {rclcpp::spin( pub_node );} );
auto sub_node = rclcpp::Node::make_shared( "sub_node" );
ServiceClient<std_srvs::srv::Empty> client( "empty_srv", sub_node );
auto sub = sub_node->create_subscription<std_msgs::msg::Empty>(
"empty_topic",
rclcpp::QoS( 1 ),
[&client]( std_msgs::msg::Empty::SharedPtr ) {
auto req = std::make_shared<std_srvs::srv::Empty::Request>( );
auto res = client.invoke( req );
} );
pub->publish( std_msgs::msg::Empty( ) );
rclcpp::spin_some( sub_node );
rclcpp::shutdown( );
srv_thread.join( );
pub_thread.join( );
ASSERT_EQ( a, 1 );
}
90 TEST( ServiceClient, can_ServiceClient_timeout )
{
rclcpp::init( 0, nullptr );
auto node = rclcpp::Node::make_shared( "test_node" );
TestServiceClient t( "bar", node );
rclcpp::spin_some( node );
bool ready = t.wait_for_service( std::chrono::milliseconds( 10 ) );
rclcpp::shutdown( );
ASSERT_EQ( ready, false );
}
1 // Copyright ( c ) 2019 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include "nav2_util/string_utils.hpp"
#include "gtest/gtest.h"
using nav2_util::split;
using nav2_util::Tokens;
23 TEST( Split, SplitFunction )
{
ASSERT_EQ( split( "", ':' ), Tokens( {""} ) );
ASSERT_EQ( split( "foo", ':' ), Tokens{"foo"} );
ASSERT_EQ( split( "foo:bar", ':' ), Tokens( {"foo", "bar"} ) );
ASSERT_EQ( split( "foo:bar:", ':' ), Tokens( {"foo", "bar", ""} ) );
ASSERT_EQ( split( ":", ':' ), Tokens( {"", ""} ) );
ASSERT_EQ( split( "foo::bar", ':' ), Tokens( {"foo", "", "bar"} ) );
ASSERT_TRUE( nav2_util::strip_leading_slash( std::string( "/hi" ) ) == std::string( "hi" ) );
}
// Copyright ( c ) 2022 Samsung Research
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_VELOCITY_SMOOTHER__VELOCITY_SMOOTHER_HPP_
#define NAV2_VELOCITY_SMOOTHER__VELOCITY_SMOOTHER_HPP_
#include <chrono>
#include <limits>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_util/odometry_utils.hpp"
namespace nav2_velocity_smoother
{
/**
* @class nav2_velocity_smoother::VelocitySmoother
* @brief This class that smooths cmd_vel velocities for robot bases
*/
36 class VelocitySmoother : public nav2_util::LifecycleNode
{
public:
/**
* @brief A constructor for nav2_velocity_smoother::VelocitySmoother
* @param options Additional options to control creation of the node.
*/
43 explicit VelocitySmoother( const rclcpp::NodeOptions & options = rclcpp::NodeOptions( ) );
/**
* @brief Destructor for nav2_velocity_smoother::VelocitySmoother
*/
48 ~VelocitySmoother( );
/**
* @brief Find the scale factor, eta, which scales axis into acceleration range
* @param v_curr current velocity
* @param v_cmd commanded velocity
* @param accel maximum acceleration
* @param decel maximum deceleration
* @return Scale factor, eta
*/
58 double findEtaConstraint(
const double v_curr, const double v_cmd,
const double accel, const double decel );
/**
* @brief Apply acceleration and scale factor constraints
* @param v_curr current velocity
* @param v_cmd commanded velocity
* @param accel maximum acceleration
* @param decel maximum deceleration
* @param eta Scale factor
* @return Velocity command
*/
71 double applyConstraints(
const double v_curr, const double v_cmd,
const double accel, const double decel, const double eta );
protected:
/**
* @brief Configures parameters and member variables
* @param state LifeCycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_configure( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Activates member variables
* @param state LifeCycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_activate( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Deactivates member variables
* @param state LifeCycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Calls clean up states and resets member variables.
* @param state LifeCycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Called when in Shutdown state
* @param state LifeCycle Node's state
* @return Success or Failure
*/
nav2_util::CallbackReturn on_shutdown( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Callback for incoming velocity commands
* @param msg Twist message
*/
void inputCommandCallback( const geometry_msgs::msg::Twist::SharedPtr msg );
/**
* @brief Main worker timer function
*/
void smootherTimer( );
/**
* @brief Dynamic reconfigure callback
* @param parameters Parameter list to change
*/
rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(
std::vector<rclcpp::Parameter> parameters );
// Network interfaces
std::unique_ptr<nav2_util::OdomSmoother> odom_smoother_;
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr
smoothed_cmd_pub_;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub_;
rclcpp::TimerBase::SharedPtr timer_;
rclcpp::Clock::SharedPtr clock_;
geometry_msgs::msg::Twist last_cmd_;
geometry_msgs::msg::Twist::SharedPtr command_;
// Parameters
double smoothing_frequency_;
double odom_duration_;
std::string odom_topic_;
bool open_loop_;
bool stopped_{true};
bool scale_velocities_;
std::vector<double> max_velocities_;
std::vector<double> min_velocities_;
std::vector<double> max_accels_;
std::vector<double> max_decels_;
std::vector<double> deadband_velocities_;
rclcpp::Duration velocity_timeout_{0, 0};
rclcpp::Time last_command_time_;
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
};
} // namespace nav2_velocity_smoother
#endif // NAV2_VELOCITY_SMOOTHER__VELOCITY_SMOOTHER_HPP_
1 // Copyright ( c ) 2022 Samsung Research
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include "nav2_velocity_smoother/velocity_smoother.hpp"
#include "rclcpp/rclcpp.hpp"
20 int main( int argc, char ** argv )
{
rclcpp::init( argc, argv );
auto node = std::make_shared<nav2_velocity_smoother::VelocitySmoother>( );
rclcpp::spin( node->get_node_base_interface( ) );
rclcpp::shutdown( );
return 0;
}
// Copyright ( c ) 2022 Samsung Research
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <chrono>
#include <limits>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "nav2_velocity_smoother/velocity_smoother.hpp"
using namespace std::chrono_literals;
using nav2_util::declare_parameter_if_not_declared;
using std::placeholders::_1;
using rcl_interfaces::msg::ParameterType;
namespace nav2_velocity_smoother
{
32 VelocitySmoother::VelocitySmoother( const rclcpp::NodeOptions & options )
: LifecycleNode( "velocity_smoother", "", options ),
last_command_time_{0, 0, get_clock( )->get_clock_type( )}
{
}
VelocitySmoother::~VelocitySmoother( )
{
if ( timer_ ) {
timer_->cancel( );
timer_.reset( );
}
}
nav2_util::CallbackReturn
VelocitySmoother::on_configure( const rclcpp_lifecycle::State & )
{
RCLCPP_INFO( get_logger( ), "Configuring velocity smoother" );
auto node = shared_from_this( );
std::string feedback_type;
double velocity_timeout_dbl;
// Smoothing metadata
declare_parameter_if_not_declared( node, "smoothing_frequency", rclcpp::ParameterValue( 20.0 ) );
declare_parameter_if_not_declared(
node, "feedback", rclcpp::ParameterValue( std::string( "OPEN_LOOP" ) ) );
declare_parameter_if_not_declared( node, "scale_velocities", rclcpp::ParameterValue( false ) );
node->get_parameter( "smoothing_frequency", smoothing_frequency_ );
node->get_parameter( "feedback", feedback_type );
node->get_parameter( "scale_velocities", scale_velocities_ );
// Kinematics
declare_parameter_if_not_declared(
node, "max_velocity", rclcpp::ParameterValue( std::vector<double>{0.50, 0.0, 2.5} ) );
declare_parameter_if_not_declared(
node, "min_velocity", rclcpp::ParameterValue( std::vector<double>{-0.50, 0.0, -2.5} ) );
declare_parameter_if_not_declared(
node, "max_accel", rclcpp::ParameterValue( std::vector<double>{2.5, 0.0, 3.2} ) );
declare_parameter_if_not_declared(
node, "max_decel", rclcpp::ParameterValue( std::vector<double>{-2.5, 0.0, -3.2} ) );
node->get_parameter( "max_velocity", max_velocities_ );
node->get_parameter( "min_velocity", min_velocities_ );
node->get_parameter( "max_accel", max_accels_ );
node->get_parameter( "max_decel", max_decels_ );
for ( unsigned int i = 0; i != max_decels_.size( ); i++ ) {
if ( max_decels_[i] > 0.0 ) {
RCLCPP_WARN(
get_logger( ),
"Positive values set of deceleration! These should be negative to slow down!" );
}
}
// Get feature parameters
declare_parameter_if_not_declared( node, "odom_topic", rclcpp::ParameterValue( "odom" ) );
declare_parameter_if_not_declared( node, "odom_duration", rclcpp::ParameterValue( 0.1 ) );
declare_parameter_if_not_declared(
node, "deadband_velocity", rclcpp::ParameterValue( std::vector<double>{0.0, 0.0, 0.0} ) );
declare_parameter_if_not_declared( node, "velocity_timeout", rclcpp::ParameterValue( 1.0 ) );
node->get_parameter( "odom_topic", odom_topic_ );
node->get_parameter( "odom_duration", odom_duration_ );
node->get_parameter( "deadband_velocity", deadband_velocities_ );
node->get_parameter( "velocity_timeout", velocity_timeout_dbl );
velocity_timeout_ = rclcpp::Duration::from_seconds( velocity_timeout_dbl );
if ( max_velocities_.size( ) != 3 || min_velocities_.size( ) != 3 ||
max_accels_.size( ) != 3 || max_decels_.size( ) != 3 || deadband_velocities_.size( ) != 3 )
{
throw std::runtime_error(
"Invalid setting of kinematic and/or deadband limits!"
" All limits must be size of 3 representing ( x, y, theta )." );
}
// Get control type
if ( feedback_type == "OPEN_LOOP" ) {
open_loop_ = true;
} else if ( feedback_type == "CLOSED_LOOP" ) {
open_loop_ = false;
odom_smoother_ = std::make_unique<nav2_util::OdomSmoother>( node, odom_duration_, odom_topic_ );
} else {
throw std::runtime_error( "Invalid feedback_type, options are OPEN_LOOP and CLOSED_LOOP." );
}
// Setup inputs / outputs
smoothed_cmd_pub_ = create_publisher<geometry_msgs::msg::Twist>( "cmd_vel_smoothed", 1 );
cmd_sub_ = create_subscription<geometry_msgs::msg::Twist>(
"cmd_vel", rclcpp::QoS( 1 ),
std::bind( &VelocitySmoother::inputCommandCallback, this, std::placeholders::_1 ) );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
VelocitySmoother::on_activate( const rclcpp_lifecycle::State & )
{
RCLCPP_INFO( get_logger( ), "Activating" );
smoothed_cmd_pub_->on_activate( );
double timer_duration_ms = 1000.0 / smoothing_frequency_;
timer_ = create_wall_timer(
std::chrono::milliseconds( static_cast<int>( timer_duration_ms ) ),
std::bind( &VelocitySmoother::smootherTimer, this ) );
dyn_params_handler_ = this->add_on_set_parameters_callback(
std::bind( &VelocitySmoother::dynamicParametersCallback, this, _1 ) );
// create bond connection
createBond( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
VelocitySmoother::on_deactivate( const rclcpp_lifecycle::State & )
{
RCLCPP_INFO( get_logger( ), "Deactivating" );
if ( timer_ ) {
timer_->cancel( );
timer_.reset( );
}
smoothed_cmd_pub_->on_deactivate( );
dyn_params_handler_.reset( );
// destroy bond connection
destroyBond( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
VelocitySmoother::on_cleanup( const rclcpp_lifecycle::State & )
{
RCLCPP_INFO( get_logger( ), "Cleaning up" );
smoothed_cmd_pub_.reset( );
odom_smoother_.reset( );
cmd_sub_.reset( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
VelocitySmoother::on_shutdown( const rclcpp_lifecycle::State & )
{
RCLCPP_INFO( get_logger( ), "Shutting down" );
return nav2_util::CallbackReturn::SUCCESS;
}
void VelocitySmoother::inputCommandCallback( const geometry_msgs::msg::Twist::SharedPtr msg )
{
command_ = msg;
last_command_time_ = now( );
}
double VelocitySmoother::findEtaConstraint(
const double v_curr, const double v_cmd, const double accel, const double decel )
{
// Exploiting vector scaling properties
const double v_component_max = accel / smoothing_frequency_;
const double v_component_min = decel / smoothing_frequency_;
const double dv = v_cmd - v_curr;
if ( dv > v_component_max ) {
return v_component_max / dv;
}
if ( dv < v_component_min ) {
return v_component_min / dv;
}
return -1.0;
}
double VelocitySmoother::applyConstraints(
const double v_curr, const double v_cmd,
const double accel, const double decel, const double eta )
{
double dv = v_cmd - v_curr;
const double v_component_max = accel / smoothing_frequency_;
const double v_component_min = decel / smoothing_frequency_;
return v_curr + std::clamp( eta * dv, v_component_min, v_component_max );
}
void VelocitySmoother::smootherTimer( )
{
auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>( );
// Check for velocity timeout. If nothing received, publish zeros to stop robot
if ( now( ) - last_command_time_ > velocity_timeout_ ) {
last_cmd_ = geometry_msgs::msg::Twist( );
if ( !stopped_ ) {
smoothed_cmd_pub_->publish( std::move( cmd_vel ) );
}
stopped_ = true;
return;
}
stopped_ = false;
// Get current velocity based on feedback type
geometry_msgs::msg::Twist current_;
if ( open_loop_ ) {
current_ = last_cmd_;
} else {
current_ = odom_smoother_->getTwist( );
}
// Apply absolute velocity restrictions to the command
command_->linear.x = std::clamp( command_->linear.x, min_velocities_[0], max_velocities_[0] );
command_->linear.y = std::clamp( command_->linear.y, min_velocities_[1], max_velocities_[1] );
command_->angular.z = std::clamp( command_->angular.z, min_velocities_[2], max_velocities_[2] );
// Find if any component is not within the acceleration constraints. If so, store the most
// significant scale factor to apply to the vector <dvx, dvy, dvw>, eta, to reduce all axes
// proportionally to follow the same direction, within change of velocity bounds.
// In case eta reduces another axis out of its own limit, apply accel constraint to guarantee
// output is within limits, even if it deviates from requested command slightly.
double eta = 1.0;
if ( scale_velocities_ ) {
double curr_eta = -1.0;
curr_eta = findEtaConstraint(
current_.linear.x, command_->linear.x, max_accels_[0], max_decels_[0] );
if ( curr_eta > 0.0 && std::fabs( 1.0 - curr_eta ) > std::fabs( 1.0 - eta ) ) {
eta = curr_eta;
}
curr_eta = findEtaConstraint(
current_.linear.y, command_->linear.y, max_accels_[1], max_decels_[1] );
if ( curr_eta > 0.0 && std::fabs( 1.0 - curr_eta ) > std::fabs( 1.0 - eta ) ) {
eta = curr_eta;
}
curr_eta = findEtaConstraint(
current_.angular.z, command_->angular.z, max_accels_[2], max_decels_[2] );
if ( curr_eta > 0.0 && std::fabs( 1.0 - curr_eta ) > std::fabs( 1.0 - eta ) ) {
eta = curr_eta;
}
}
cmd_vel->linear.x = applyConstraints(
current_.linear.x, command_->linear.x, max_accels_[0], max_decels_[0], eta );
cmd_vel->linear.y = applyConstraints(
current_.linear.y, command_->linear.y, max_accels_[1], max_decels_[1], eta );
cmd_vel->angular.z = applyConstraints(
current_.angular.z, command_->angular.z, max_accels_[2], max_decels_[2], eta );
// If open loop, assume we achieved it
if ( open_loop_ ) {
last_cmd_ = *cmd_vel;
}
// Apply deadband restrictions & publish
cmd_vel->linear.x = fabs( cmd_vel->linear.x ) < deadband_velocities_[0] ? 0.0 : cmd_vel->linear.x;
cmd_vel->linear.y = fabs( cmd_vel->linear.y ) < deadband_velocities_[1] ? 0.0 : cmd_vel->linear.y;
cmd_vel->angular.z = fabs( cmd_vel->angular.z ) <
deadband_velocities_[2] ? 0.0 : cmd_vel->angular.z;
smoothed_cmd_pub_->publish( std::move( cmd_vel ) );
}
rcl_interfaces::msg::SetParametersResult
VelocitySmoother::dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters )
{
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
for ( auto parameter : parameters ) {
const auto & type = parameter.get_type( );
const auto & name = parameter.get_name( );
if ( type == ParameterType::PARAMETER_DOUBLE ) {
if ( name == "smoothing_frequency" ) {
smoothing_frequency_ = parameter.as_double( );
if ( timer_ ) {
timer_->cancel( );
timer_.reset( );
}
double timer_duration_ms = 1000.0 / smoothing_frequency_;
timer_ = create_wall_timer(
std::chrono::milliseconds( static_cast<int>( timer_duration_ms ) ),
std::bind( &VelocitySmoother::smootherTimer, this ) );
} else if ( name == "velocity_timeout" ) {
velocity_timeout_ = rclcpp::Duration::from_seconds( parameter.as_double( ) );
} else if ( name == "odom_duration" ) {
odom_duration_ = parameter.as_double( );
odom_smoother_ =
std::make_unique<nav2_util::OdomSmoother>(
shared_from_this( ), odom_duration_, odom_topic_ );
}
} else if ( type == ParameterType::PARAMETER_DOUBLE_ARRAY ) {
if ( parameter.as_double_array( ).size( ) != 3 ) {
RCLCPP_WARN( get_logger( ), "Invalid size of parameter %s. Must be size 3", name.c_str( ) );
result.successful = false;
break;
}
if ( name == "max_velocity" ) {
max_velocities_ = parameter.as_double_array( );
} else if ( name == "min_velocity" ) {
min_velocities_ = parameter.as_double_array( );
} else if ( name == "max_accel" ) {
max_accels_ = parameter.as_double_array( );
} else if ( name == "max_decel" ) {
max_decels_ = parameter.as_double_array( );
} else if ( name == "deadband_velocity" ) {
deadband_velocities_ = parameter.as_double_array( );
}
} else if ( type == ParameterType::PARAMETER_STRING ) {
if ( name == "feedback" ) {
if ( parameter.as_string( ) == "OPEN_LOOP" ) {
open_loop_ = true;
odom_smoother_.reset( );
} else if ( parameter.as_string( ) == "CLOSED_LOOP" ) {
open_loop_ = false;
odom_smoother_ =
std::make_unique<nav2_util::OdomSmoother>(
shared_from_this( ), odom_duration_, odom_topic_ );
} else {
RCLCPP_WARN(
get_logger( ), "Invalid feedback_type, options are OPEN_LOOP and CLOSED_LOOP." );
result.successful = false;
break;
}
} else if ( name == "odom_topic" ) {
odom_topic_ = parameter.as_string( );
odom_smoother_ =
std::make_unique<nav2_util::OdomSmoother>(
shared_from_this( ), odom_duration_, odom_topic_ );
}
}
}
return result;
}
} // namespace nav2_velocity_smoother
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE( nav2_velocity_smoother::VelocitySmoother )
1 // Copyright ( c ) 2022 Samsung Research
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <math.h>
#include <memory>
#include <string>
#include <vector>
#include <limits>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_velocity_smoother/velocity_smoother.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "geometry_msgs/msg/twist.hpp"
using namespace std::chrono_literals;
29 class RclCppFixture
{
public:
32 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
33 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
37 class VelSmootherShim : public nav2_velocity_smoother::VelocitySmoother
{
public:
40 VelSmootherShim( )
: VelocitySmoother( ) {}
42 void configure( const rclcpp_lifecycle::State & state ) {this->on_configure( state );}
43 void activate( const rclcpp_lifecycle::State & state ) {this->on_activate( state );}
44 void deactivate( const rclcpp_lifecycle::State & state ) {this->on_deactivate( state );}
45 void cleanup( const rclcpp_lifecycle::State & state ) {this->on_cleanup( state );}
46 void shutdown( const rclcpp_lifecycle::State & state ) {this->on_shutdown( state );}
48 bool isOdomSmoother( ) {return odom_smoother_ ? true : false;}
49 bool hasCommandMsg( ) {return last_command_time_.nanoseconds( ) != 0;}
50 geometry_msgs::msg::Twist::SharedPtr lastCommandMsg( ) {return command_;}
52 void sendCommandMsg( geometry_msgs::msg::Twist::SharedPtr msg ) {inputCommandCallback( msg );}
};
55 TEST( VelocitySmootherTest, openLoopTestTimer )
{
auto smoother =
std::make_shared<VelSmootherShim>( );
std::vector<double> deadbands{0.2, 0.0, 0.0};
smoother->declare_parameter( "scale_velocities", rclcpp::ParameterValue( true ) );
smoother->set_parameter( rclcpp::Parameter( "scale_velocities", true ) );
smoother->declare_parameter( "deadband_velocity", rclcpp::ParameterValue( deadbands ) );
smoother->set_parameter( rclcpp::Parameter( "deadband_velocity", deadbands ) );
rclcpp_lifecycle::State state;
smoother->configure( state );
smoother->activate( state );
std::vector<double> linear_vels;
auto subscription = smoother->create_subscription<geometry_msgs::msg::Twist>(
"cmd_vel_smoothed",
1,
[&]( geometry_msgs::msg::Twist::SharedPtr msg ) {
linear_vels.push_back( msg->linear.x );
} );
// Send a velocity command
auto cmd = std::make_shared<geometry_msgs::msg::Twist>( );
cmd->linear.x = 1.0; // Max is 0.5, so should threshold
smoother->sendCommandMsg( cmd );
// Process velocity smoothing and send updated odometry based on commands
auto start = smoother->now( );
while ( smoother->now( ) - start < 1.5s ) {
rclcpp::spin_some( smoother->get_node_base_interface( ) );
}
// Sanity check we have the approximately right number of messages for the timespan and timeout
EXPECT_GT( linear_vels.size( ), 19u );
EXPECT_LT( linear_vels.size( ), 30u );
// Should have last command be a stop since we timed out the command stream
EXPECT_EQ( linear_vels.back( ), 0.0 );
// From deadband, first few should be 0 until above 0.2
for ( unsigned int i = 0; i != linear_vels.size( ); i++ ) {
if ( linear_vels[i] != 0 ) {
EXPECT_GT( linear_vels[i], 0.2 );
break;
}
}
// Process to make sure stops at limit in velocity,
// doesn't exceed acceleration
for ( unsigned int i = 0; i != linear_vels.size( ); i++ ) {
EXPECT_TRUE( linear_vels[i] <= 0.5 );
}
}
109 TEST( VelocitySmootherTest, approxClosedLoopTestTimer )
{
auto smoother =
std::make_shared<VelSmootherShim>( );
smoother->declare_parameter( "feedback", rclcpp::ParameterValue( std::string( "CLOSED_LOOP" ) ) );
smoother->set_parameter( rclcpp::Parameter( "feedback", std::string( "CLOSED_LOOP" ) ) );
rclcpp_lifecycle::State state;
smoother->configure( state );
smoother->activate( state );
std::vector<double> linear_vels;
auto subscription = smoother->create_subscription<geometry_msgs::msg::Twist>(
"cmd_vel_smoothed",
1,
[&]( geometry_msgs::msg::Twist::SharedPtr msg ) {
linear_vels.push_back( msg->linear.x );
} );
auto odom_pub = smoother->create_publisher<nav_msgs::msg::Odometry>( "odom", 1 );
odom_pub->on_activate( );
nav_msgs::msg::Odometry odom_msg;
odom_msg.header.frame_id = "odom";
odom_msg.child_frame_id = "base_link";
// Fill buffer with 0 twisted-commands
for ( unsigned int i = 0; i != 30; i++ ) {
odom_msg.header.stamp = smoother->now( ) + rclcpp::Duration::from_seconds( i * 0.01 );
odom_pub->publish( odom_msg );
}
// Send a velocity command
auto cmd = std::make_shared<geometry_msgs::msg::Twist>( );
cmd->linear.x = 1.0; // Max is 0.5, so should threshold
smoother->sendCommandMsg( cmd );
// Process velocity smoothing and send updated odometry based on commands
auto start = smoother->now( );
while ( smoother->now( ) - start < 1.5s ) {
odom_msg.header.stamp = smoother->now( );
if ( linear_vels.size( ) > 0 ) {
odom_msg.twist.twist.linear.x = linear_vels.back( );
}
odom_pub->publish( odom_msg );
rclcpp::spin_some( smoother->get_node_base_interface( ) );
}
// Sanity check we have the approximately right number of messages for the timespan and timeout
EXPECT_GT( linear_vels.size( ), 19u );
EXPECT_LT( linear_vels.size( ), 30u );
// Should have last command be a stop since we timed out the command stream
EXPECT_EQ( linear_vels.back( ), 0.0 );
// Process to make sure stops at limit in velocity,
// doesn't exceed acceleration
for ( unsigned int i = 0; i != linear_vels.size( ); i++ ) {
if ( i > 0 ) {
double diff = linear_vels[i] - linear_vels[i - 1];
EXPECT_LT( diff, 0.126 ); // default accel of 0.5 / 20 hz = 0.125
}
EXPECT_TRUE( linear_vels[i] <= 0.5 );
}
}
174 TEST( VelocitySmootherTest, testfindEtaConstraint )
{
auto smoother =
std::make_shared<VelSmootherShim>( );
rclcpp_lifecycle::State state;
// default frequency is 20.0
smoother->configure( state );
// In range
EXPECT_EQ( smoother->findEtaConstraint( 1.0, 1.0, 1.5, -2.0 ), -1 );
EXPECT_EQ( smoother->findEtaConstraint( 0.5, 0.55, 1.5, -2.0 ), -1 );
EXPECT_EQ( smoother->findEtaConstraint( 0.5, 0.45, 1.5, -2.0 ), -1 );
// Too high
EXPECT_EQ( smoother->findEtaConstraint( 1.0, 2.0, 1.5, -2.0 ), 0.075 );
// Too low
EXPECT_EQ( smoother->findEtaConstraint( 1.0, 0.0, 1.5, -2.0 ), 0.1 );
// In a more realistic situation accelerating linear axis
EXPECT_NEAR( smoother->findEtaConstraint( 0.40, 0.50, 1.5, -2.0 ), 0.75, 0.001 );
}
195 TEST( VelocitySmootherTest, testapplyConstraints )
{
auto smoother =
std::make_shared<VelSmootherShim>( );
rclcpp_lifecycle::State state;
// default frequency is 20.0
smoother->configure( state );
double no_eta = 1.0;
// Apply examples from testfindEtaConstraint
// In range, so no eta or acceleration limit impact
EXPECT_EQ( smoother->applyConstraints( 1.0, 1.0, 1.5, -2.0, no_eta ), 1.0 );
EXPECT_EQ( smoother->applyConstraints( 0.5, 0.55, 1.5, -2.0, no_eta ), 0.55 );
EXPECT_EQ( smoother->applyConstraints( 0.5, 0.45, 1.5, -2.0, no_eta ), 0.45 );
// Too high, without eta
EXPECT_NEAR( smoother->applyConstraints( 1.0, 2.0, 1.5, -2.0, no_eta ), 1.075, 0.01 );
// Too high, with eta applied on its own axis
EXPECT_NEAR( smoother->applyConstraints( 1.0, 2.0, 1.5, -2.0, 0.075 ), 1.075, 0.01 );
// On another virtual axis that is OK
EXPECT_NEAR( smoother->applyConstraints( 0.5, 0.55, 1.5, -2.0, 0.075 ), 0.503, 0.01 );
// In a more realistic situation, applied to angular
EXPECT_NEAR( smoother->applyConstraints( 0.8, 1.0, 3.2, -3.2, 0.75 ), 1.075, 0.95 );
}
220 TEST( VelocitySmootherTest, testCommandCallback )
{
auto smoother =
std::make_shared<VelSmootherShim>( );
rclcpp_lifecycle::State state;
smoother->configure( state );
smoother->activate( state );
auto pub = smoother->create_publisher<geometry_msgs::msg::Twist>( "cmd_vel", 1 );
pub->on_activate( );
auto msg = std::make_unique<geometry_msgs::msg::Twist>( );
msg->linear.x = 100.0;
pub->publish( std::move( msg ) );
rclcpp::spin_some( smoother->get_node_base_interface( ) );
EXPECT_TRUE( smoother->hasCommandMsg( ) );
EXPECT_EQ( smoother->lastCommandMsg( )->linear.x, 100.0 );
}
239 TEST( VelocitySmootherTest, testClosedLoopSub )
{
auto smoother =
std::make_shared<VelSmootherShim>( );
smoother->declare_parameter( "feedback", rclcpp::ParameterValue( std::string( "OPEN_LOOP" ) ) );
smoother->set_parameter( rclcpp::Parameter( "feedback", std::string( "CLOSED_LOOP" ) ) );
rclcpp_lifecycle::State state;
smoother->configure( state );
EXPECT_TRUE( smoother->isOdomSmoother( ) );
}
250 TEST( VelocitySmootherTest, testInvalidParams )
{
auto smoother =
std::make_shared<VelSmootherShim>( );
std::vector<double> max_vels{0.0, 0.0}; // invalid size
smoother->declare_parameter( "max_velocity", rclcpp::ParameterValue( max_vels ) );
rclcpp_lifecycle::State state;
EXPECT_THROW( smoother->configure( state ), std::runtime_error );
smoother->set_parameter( rclcpp::Parameter( "feedback", std::string( "LAWLS" ) ) );
EXPECT_THROW( smoother->configure( state ), std::runtime_error );
}
263 TEST( VelocitySmootherTest, testDynamicParameter )
{
auto smoother =
std::make_shared<VelSmootherShim>( );
rclcpp_lifecycle::State state;
smoother->configure( state );
smoother->activate( state );
EXPECT_FALSE( smoother->isOdomSmoother( ) );
auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
smoother->get_node_base_interface( ), smoother->get_node_topics_interface( ),
smoother->get_node_graph_interface( ),
smoother->get_node_services_interface( ) );
std::vector<double> max_vel{10.0, 10.0, 10.0};
std::vector<double> min_vel{0.0, 0.0, 0.0};
std::vector<double> max_accel{10.0, 10.0, 10.0};
std::vector<double> min_accel{0.0, 0.0, 0.0};
std::vector<double> deadband{0.0, 0.0, 0.0};
std::vector<double> bad_test{0.0, 0.0};
auto results = rec_param->set_parameters_atomically(
{rclcpp::Parameter( "smoothing_frequency", 100.0 ),
rclcpp::Parameter( "feedback", std::string( "CLOSED_LOOP" ) ),
rclcpp::Parameter( "scale_velocities", true ),
rclcpp::Parameter( "max_velocity", max_vel ),
rclcpp::Parameter( "min_velocity", min_vel ),
rclcpp::Parameter( "max_accel", max_accel ),
rclcpp::Parameter( "max_decel", min_accel ),
rclcpp::Parameter( "odom_topic", std::string( "TEST" ) ),
rclcpp::Parameter( "odom_duration", 2.0 ),
rclcpp::Parameter( "velocity_timeout", 4.0 ),
rclcpp::Parameter( "deadband_velocity", deadband )} );
rclcpp::spin_until_future_complete(
smoother->get_node_base_interface( ),
results );
EXPECT_EQ( smoother->get_parameter( "smoothing_frequency" ).as_double( ), 100.0 );
EXPECT_EQ( smoother->get_parameter( "feedback" ).as_string( ), std::string( "CLOSED_LOOP" ) );
EXPECT_EQ( smoother->get_parameter( "scale_velocities" ).as_bool( ), true );
EXPECT_EQ( smoother->get_parameter( "max_velocity" ).as_double_array( ), max_vel );
EXPECT_EQ( smoother->get_parameter( "min_velocity" ).as_double_array( ), min_vel );
EXPECT_EQ( smoother->get_parameter( "max_accel" ).as_double_array( ), max_accel );
EXPECT_EQ( smoother->get_parameter( "max_decel" ).as_double_array( ), min_accel );
EXPECT_EQ( smoother->get_parameter( "odom_topic" ).as_string( ), std::string( "TEST" ) );
EXPECT_EQ( smoother->get_parameter( "odom_duration" ).as_double( ), 2.0 );
EXPECT_EQ( smoother->get_parameter( "velocity_timeout" ).as_double( ), 4.0 );
EXPECT_EQ( smoother->get_parameter( "deadband_velocity" ).as_double_array( ), deadband );
// Test reverting
results = rec_param->set_parameters_atomically(
{rclcpp::Parameter( "feedback", std::string( "OPEN_LOOP" ) )} );
rclcpp::spin_until_future_complete(
smoother->get_node_base_interface( ), results );
EXPECT_EQ( smoother->get_parameter( "feedback" ).as_string( ), std::string( "OPEN_LOOP" ) );
// Test invalid change
results = rec_param->set_parameters_atomically(
{rclcpp::Parameter( "feedback", std::string( "LAWLS" ) )} );
rclcpp::spin_until_future_complete( smoother->get_node_base_interface( ), results );
EXPECT_FALSE( results.get( ).successful );
// Test invalid size
results = rec_param->set_parameters_atomically(
{rclcpp::Parameter( "max_velocity", bad_test )} );
rclcpp::spin_until_future_complete( smoother->get_node_base_interface( ), results );
EXPECT_FALSE( results.get( ).successful );
// test full state after major changes
smoother->deactivate( state );
smoother->cleanup( state );
smoother->shutdown( state );
smoother.reset( );
}
/*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#ifndef NAV2_VOXEL_GRID__VOXEL_GRID_HPP_
#define NAV2_VOXEL_GRID__VOXEL_GRID_HPP_
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <stdint.h>
#include <math.h>
#include <limits.h>
#include <algorithm>
#include "rclcpp/rclcpp.hpp"
/**
* @class VoxelGrid
* @brief A 3D grid structure that stores points as an integer array.
* X and Y index the array and Z selects which bit of the integer
* is used giving a limit of 16 vertical cells.
*/
namespace nav2_voxel_grid
{
enum VoxelStatus
{
FREE = 0,
UNKNOWN = 1,
MARKED = 2,
};
65 class VoxelGrid
{
public:
/**
* @brief Constructor for a voxel grid
* @param size_x The x size of the grid
* @param size_y The y size of the grid
* @param size_z The z size of the grid, only sizes <= 16 are supported
*/
74 VoxelGrid( unsigned int size_x, unsigned int size_y, unsigned int size_z );
76 ~VoxelGrid( );
/**
* @brief Resizes a voxel grid to the desired size
* @param size_x The x size of the grid
* @param size_y The y size of the grid
* @param size_z The z size of the grid, only sizes <= 16 are supported
*/
84 void resize( unsigned int size_x, unsigned int size_y, unsigned int size_z );
86 void reset( );
87 uint32_t * getData( ) {return data_;}
89 inline void markVoxel( unsigned int x, unsigned int y, unsigned int z )
{
if ( x >= size_x_ || y >= size_y_ || z >= size_z_ ) {
RCLCPP_DEBUG( logger, "Error, voxel out of bounds.\n" );
return;
}
uint32_t full_mask = ( ( uint32_t )1 << z << 16 ) | ( 1 << z );
data_[y * size_x_ + x] |= full_mask; // clear unknown and mark cell
}
99 inline bool markVoxelInMap(
unsigned int x, unsigned int y, unsigned int z,
unsigned int marked_threshold )
{
if ( x >= size_x_ || y >= size_y_ || z >= size_z_ ) {
RCLCPP_DEBUG( logger, "Error, voxel out of bounds.\n" );
return false;
}
int index = y * size_x_ + x;
uint32_t * col = &data_[index];
uint32_t full_mask = ( ( uint32_t )1 << z << 16 ) | ( 1 << z );
*col |= full_mask; // clear unknown and mark cell
unsigned int marked_bits = *col >> 16;
// make sure the number of bits in each is below our thesholds
return !bitsBelowThreshold( marked_bits, marked_threshold );
}
119 inline void clearVoxel( unsigned int x, unsigned int y, unsigned int z )
{
if ( x >= size_x_ || y >= size_y_ || z >= size_z_ ) {
RCLCPP_DEBUG( logger, "Error, voxel out of bounds.\n" );
return;
}
uint32_t full_mask = ( ( uint32_t )1 << z << 16 ) | ( 1 << z );
data_[y * size_x_ + x] &= ~( full_mask ); // clear unknown and clear cell
}
129 inline void clearVoxelColumn( unsigned int index )
{
assert( index < size_x_ * size_y_ );
data_[index] = 0;
}
135 inline void clearVoxelInMap( unsigned int x, unsigned int y, unsigned int z )
{
if ( x >= size_x_ || y >= size_y_ || z >= size_z_ ) {
RCLCPP_DEBUG( logger, "Error, voxel out of bounds.\n" );
return;
}
int index = y * size_x_ + x;
uint32_t * col = &data_[index];
uint32_t full_mask = ( ( uint32_t )1 << z << 16 ) | ( 1 << z );
*col &= ~( full_mask ); // clear unknown and clear cell
unsigned int unknown_bits = uint16_t( *col >> 16 ) ^ uint16_t( *col );
unsigned int marked_bits = *col >> 16;
// make sure the number of bits in each is below our thesholds
if ( bitsBelowThreshold( unknown_bits, 1 ) && bitsBelowThreshold( marked_bits, 1 ) ) {
costmap[index] = 0;
}
}
155 inline bool bitsBelowThreshold( unsigned int n, unsigned int bit_threshold )
{
unsigned int bit_count;
for ( bit_count = 0; n; ) {
++bit_count;
if ( bit_count > bit_threshold ) {
return false;
}
n &= n - 1; // clear the least significant bit set
}
return true;
}
168 static inline unsigned int numBits( unsigned int n )
{
unsigned int bit_count;
for ( bit_count = 0; n; ++bit_count ) {
n &= n - 1; // clear the least significant bit set
}
return bit_count;
}
177 static VoxelStatus getVoxel(
unsigned int x, unsigned int y, unsigned int z,
179 unsigned int size_x, unsigned int size_y, unsigned int size_z, const uint32_t * data )
{
if ( x >= size_x || y >= size_y || z >= size_z ) {
return UNKNOWN;
}
uint32_t full_mask = ( ( uint32_t )1 << z << 16 ) | ( 1 << z );
uint32_t result = data[y * size_x + x] & full_mask;
unsigned int bits = numBits( result );
// known marked: 11 = 2 bits, unknown: 01 = 1 bit, known free: 00 = 0 bits
if ( bits < 2 ) {
if ( bits < 1 ) {
return FREE;
}
return UNKNOWN;
}
return MARKED;
}
198 void markVoxelLine(
double x0, double y0, double z0, double x1, double y1, double z1,
unsigned int max_length = UINT_MAX );
201 void clearVoxelLine(
double x0, double y0, double z0, double x1, double y1, double z1,
unsigned int max_length = UINT_MAX, unsigned int min_length = 0 );
204 void clearVoxelLineInMap(
double x0, double y0, double z0, double x1, double y1, double z1, unsigned char * map_2d,
unsigned int unknown_threshold, unsigned int mark_threshold,
unsigned char free_cost = 0, unsigned char unknown_cost = 255,
unsigned int max_length = UINT_MAX, unsigned int min_length = 0 );
210 VoxelStatus getVoxel( unsigned int x, unsigned int y, unsigned int z );
// Are there any obstacles at that ( x, y ) location in the grid?
213 VoxelStatus getVoxelColumn(
unsigned int x, unsigned int y,
unsigned int unknown_threshold = 0, unsigned int marked_threshold = 0 );
217 void printVoxelGrid( );
218 void printColumnGrid( );
219 unsigned int sizeX( );
220 unsigned int sizeY( );
221 unsigned int sizeZ( );
template<class ActionType>
inline void raytraceLine(
ActionType at, double x0, double y0, double z0,
double x1, double y1, double z1, unsigned int max_length = UINT_MAX,
unsigned int min_length = 0 )
{
// we need to chose how much to scale our dominant dimension, based on the
// maximum length of the line
double dist = sqrt( ( x0 - x1 ) * ( x0 - x1 ) + ( y0 - y1 ) * ( y0 - y1 ) + ( z0 - z1 ) * ( z0 - z1 ) );
if ( ( unsigned int )( dist ) < min_length ) {
return;
}
double scale, min_x0, min_y0, min_z0;
236 if ( dist > 0.0 ) {
scale = std::min( 1.0, max_length / dist );
// Updating starting point to the point at distance min_length from the initial point
min_x0 = x0 + ( x1 - x0 ) / dist * min_length;
min_y0 = y0 + ( y1 - y0 ) / dist * min_length;
min_z0 = z0 + ( z1 - z0 ) / dist * min_length;
} else {
// dist can be 0 if [x0, y0, z0]==[x1, y1, z1].
// In this case only this voxel should be processed.
246 scale = 1.0;
247 min_x0 = x0;
min_y0 = y0;
min_z0 = z0;
}
int dx = int( x1 ) - int( min_x0 ); // NOLINT
int dy = int( y1 ) - int( min_y0 ); // NOLINT
int dz = int( z1 ) - int( min_z0 ); // NOLINT
unsigned int abs_dx = abs( dx );
unsigned int abs_dy = abs( dy );
unsigned int abs_dz = abs( dz );
int offset_dx = sign( dx );
int offset_dy = sign( dy ) * size_x_;
int offset_dz = sign( dz );
unsigned int z_mask = ( ( 1 << 16 ) | 1 ) << ( unsigned int )min_z0;
unsigned int offset = ( unsigned int )min_y0 * size_x_ + ( unsigned int )min_x0;
GridOffset grid_off( offset );
ZOffset z_off( z_mask );
// is x dominant
if ( abs_dx >= max( abs_dy, abs_dz ) ) {
int error_y = abs_dx / 2;
int error_z = abs_dx / 2;
bresenham3D(
at, grid_off, grid_off, z_off, abs_dx, abs_dy, abs_dz, error_y, error_z,
offset_dx, offset_dy, offset_dz, offset, z_mask, ( unsigned int )( scale * abs_dx ) );
return;
}
// y is dominant
if ( abs_dy >= abs_dz ) {
int error_x = abs_dy / 2;
int error_z = abs_dy / 2;
bresenham3D(
at, grid_off, grid_off, z_off, abs_dy, abs_dx, abs_dz, error_x, error_z,
offset_dy, offset_dx, offset_dz, offset, z_mask, ( unsigned int )( scale * abs_dy ) );
return;
}
// otherwise, z is dominant
int error_x = abs_dz / 2;
int error_y = abs_dz / 2;
bresenham3D(
at, z_off, grid_off, grid_off, abs_dz, abs_dx, abs_dy, error_x, error_y, offset_dz,
offset_dx, offset_dy, offset, z_mask, ( unsigned int )( scale * abs_dz ) );
}
private:
// the real work is done here... 3D bresenham implementation
template<class ActionType, class OffA, class OffB, class OffC>
inline void bresenham3D(
ActionType at, OffA off_a, OffB off_b, OffC off_c,
unsigned int abs_da, unsigned int abs_db, unsigned int abs_dc,
int error_b, int error_c, int offset_a, int offset_b, int offset_c, unsigned int & offset,
unsigned int & z_mask, unsigned int max_length = UINT_MAX )
{
unsigned int end = std::min( max_length, abs_da );
for ( unsigned int i = 0; i < end; ++i ) {
at( offset, z_mask );
off_a( offset_a );
error_b += abs_db;
error_c += abs_dc;
if ( ( unsigned int )error_b >= abs_da ) {
off_b( offset_b );
error_b -= abs_da;
}
if ( ( unsigned int )error_c >= abs_da ) {
off_c( offset_c );
error_c -= abs_da;
}
}
at( offset, z_mask );
}
inline int sign( int i )
{
return i > 0 ? 1 : -1;
}
inline unsigned int max( unsigned int x, unsigned int y )
{
return x > y ? x : y;
}
unsigned int size_x_, size_y_, size_z_;
uint32_t * data_;
unsigned char * costmap;
rclcpp::Logger logger;
// Aren't functors so much fun... used to recreate the Bresenham macro Eric
// wrote in the original version, but in "proper" c++
class MarkVoxel
{
public:
explicit MarkVoxel( uint32_t * data )
: data_( data ) {}
inline void operator( )( unsigned int offset, unsigned int z_mask )
{
data_[offset] |= z_mask; // clear unknown and mark cell
}
private:
uint32_t * data_;
};
class ClearVoxel
{
public:
explicit ClearVoxel( uint32_t * data )
: data_( data ) {}
inline void operator( )( unsigned int offset, unsigned int z_mask )
{
data_[offset] &= ~( z_mask ); // clear unknown and clear cell
}
private:
uint32_t * data_;
};
class ClearVoxelInMap
{
public:
ClearVoxelInMap(
uint32_t * data, unsigned char * costmap,
unsigned int unknown_clear_threshold, unsigned int marked_clear_threshold,
unsigned char free_cost = 0, unsigned char unknown_cost = 255 )
: data_( data ), costmap_( costmap ),
unknown_clear_threshold_( unknown_clear_threshold ), marked_clear_threshold_(
marked_clear_threshold ),
free_cost_( free_cost ), unknown_cost_( unknown_cost )
{
}
inline void operator( )( unsigned int offset, unsigned int z_mask )
{
uint32_t * col = &data_[offset];
*col &= ~( z_mask ); // clear unknown and clear cell
unsigned int unknown_bits = uint16_t( *col >> 16 ) ^ uint16_t( *col );
unsigned int marked_bits = *col >> 16;
// make sure the number of bits in each is below our thesholds
if ( bitsBelowThreshold( marked_bits, marked_clear_threshold_ ) ) {
if ( bitsBelowThreshold( unknown_bits, unknown_clear_threshold_ ) ) {
costmap_[offset] = free_cost_;
} else {
costmap_[offset] = unknown_cost_;
}
}
}
private:
inline bool bitsBelowThreshold( unsigned int n, unsigned int bit_threshold )
{
unsigned int bit_count;
for ( bit_count = 0; n; ) {
++bit_count;
if ( bit_count > bit_threshold ) {
return false;
}
n &= n - 1; // clear the least significant bit set
}
return true;
}
uint32_t * data_;
unsigned char * costmap_;
unsigned int unknown_clear_threshold_, marked_clear_threshold_;
unsigned char free_cost_, unknown_cost_;
};
class GridOffset
{
public:
explicit GridOffset( unsigned int & offset )
: offset_( offset ) {}
inline void operator( )( int offset_val )
{
offset_ += offset_val;
}
private:
unsigned int & offset_;
};
class ZOffset
{
public:
explicit ZOffset( unsigned int & z_mask )
: z_mask_( z_mask ) {}
inline void operator( )( int offset_val )
{
offset_val > 0 ? z_mask_ <<= 1 : z_mask_ >>= 1;
}
private:
unsigned int & z_mask_;
};
};
} // namespace nav2_voxel_grid
#endif // NAV2_VOXEL_GRID__VOXEL_GRID_HPP_
1 /*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#include <nav2_voxel_grid/voxel_grid.hpp>
namespace nav2_voxel_grid
{
41 VoxelGrid::VoxelGrid( unsigned int size_x, unsigned int size_y, unsigned int size_z )
: logger( rclcpp::get_logger( "voxel_grid" ) )
{
size_x_ = size_x;
size_y_ = size_y;
size_z_ = size_z;
if ( size_z_ > 16 ) {
RCLCPP_INFO(
logger, "Error, this implementation can only support up to 16 z values ( %d )",
size_z_ );
size_z_ = 16;
}
data_ = new uint32_t[size_x_ * size_y_];
uint32_t unknown_col = ~( ( uint32_t )0 ) >> 16;
uint32_t * col = data_;
for ( unsigned int i = 0; i < size_x_ * size_y_; ++i ) {
*col = unknown_col;
++col;
}
}
64 void VoxelGrid::resize( unsigned int size_x, unsigned int size_y, unsigned int size_z )
{
// if we're not actually changing the size, we can just reset things
if ( size_x == size_x_ && size_y == size_y_ && size_z == size_z_ ) {
reset( );
return;
}
delete[] data_;
size_x_ = size_x;
size_y_ = size_y;
size_z_ = size_z;
if ( size_z_ > 16 ) {
RCLCPP_INFO(
logger, "Error, this implementation can only support up to 16 z values ( %d )",
size_z );
size_z_ = 16;
}
data_ = new uint32_t[size_x_ * size_y_];
uint32_t unknown_col = ~( ( uint32_t )0 ) >> 16;
uint32_t * col = data_;
for ( unsigned int i = 0; i < size_x_ * size_y_; ++i ) {
*col = unknown_col;
++col;
}
}
93 VoxelGrid::~VoxelGrid( )
{
delete[] data_;
}
98 void VoxelGrid::reset( )
{
uint32_t unknown_col = ~( ( uint32_t )0 ) >> 16;
uint32_t * col = data_;
for ( unsigned int i = 0; i < size_x_ * size_y_; ++i ) {
*col = unknown_col;
++col;
}
}
108 void VoxelGrid::markVoxelLine(
double x0, double y0, double z0, double x1, double y1, double z1,
unsigned int max_length )
{
if ( x0 >= size_x_ || y0 >= size_y_ || z0 >= size_z_ || x1 >= size_x_ || y1 >= size_y_ ||
z1 >= size_z_ )
{
RCLCPP_DEBUG(
logger,
"Error, line endpoint out of bounds. "
"( %.2f, %.2f, %.2f ) to ( %.2f, %.2f, %.2f ), size: ( %d, %d, %d )",
x0, y0, z0, x1, y1, z1, size_x_, size_y_, size_z_ );
return;
}
MarkVoxel mv( data_ );
raytraceLine( mv, x0, y0, z0, x1, y1, z1, max_length );
}
127 void VoxelGrid::clearVoxelLine(
double x0, double y0, double z0, double x1, double y1, double z1,
unsigned int max_length, unsigned int min_length )
{
if ( x0 >= size_x_ || y0 >= size_y_ || z0 >= size_z_ || x1 >= size_x_ || y1 >= size_y_ ||
z1 >= size_z_ )
{
RCLCPP_DEBUG(
logger,
"Error, line endpoint out of bounds. "
"( %.2f, %.2f, %.2f ) to ( %.2f, %.2f, %.2f ), size: ( %d, %d, %d )",
x0, y0, z0, x1, y1, z1, size_x_, size_y_, size_z_ );
return;
}
ClearVoxel cv( data_ );
raytraceLine( cv, x0, y0, z0, x1, y1, z1, max_length, min_length );
}
146 void VoxelGrid::clearVoxelLineInMap(
double x0, double y0, double z0, double x1, double y1, double z1, unsigned char * map_2d,
unsigned int unknown_threshold, unsigned int mark_threshold, unsigned char free_cost,
unsigned char unknown_cost, unsigned int max_length, unsigned int min_length )
{
costmap = map_2d;
if ( map_2d == NULL ) {
clearVoxelLine( x0, y0, z0, x1, y1, z1, max_length, min_length );
return;
}
if ( x0 >= size_x_ || y0 >= size_y_ || z0 >= size_z_ || x1 >= size_x_ || y1 >= size_y_ ||
z1 >= size_z_ )
{
RCLCPP_DEBUG(
logger,
"Error, line endpoint out of bounds. "
"( %.2f, %.2f, %.2f ) to ( %.2f, %.2f, %.2f ), size: ( %d, %d, %d )",
x0, y0, z0, x1, y1, z1, size_x_, size_y_, size_z_ );
return;
}
ClearVoxelInMap cvm( data_, costmap, unknown_threshold, mark_threshold, free_cost, unknown_cost );
raytraceLine( cvm, x0, y0, z0, x1, y1, z1, max_length, min_length );
}
172 VoxelStatus VoxelGrid::getVoxel( unsigned int x, unsigned int y, unsigned int z )
{
if ( x >= size_x_ || y >= size_y_ || z >= size_z_ ) {
RCLCPP_DEBUG( logger, "Error, voxel out of bounds. ( %d, %d, %d )\n", x, y, z );
return UNKNOWN;
}
uint32_t full_mask = ( ( uint32_t )1 << z << 16 ) | ( 1 << z );
uint32_t result = data_[y * size_x_ + x] & full_mask;
unsigned int bits = numBits( result );
// known marked: 11 = 2 bits, unknown: 01 = 1 bit, known free: 00 = 0 bits
if ( bits < 2 ) {
if ( bits < 1 ) {
return FREE;
}
return UNKNOWN;
}
return MARKED;
}
194 VoxelStatus VoxelGrid::getVoxelColumn(
unsigned int x, unsigned int y,
unsigned int unknown_threshold, unsigned int marked_threshold )
{
if ( x >= size_x_ || y >= size_y_ ) {
RCLCPP_DEBUG( logger, "Error, voxel out of bounds. ( %d, %d )\n", x, y );
return UNKNOWN;
}
uint32_t * col = &data_[y * size_x_ + x];
unsigned int unknown_bits = uint16_t( *col >> 16 ) ^ uint16_t( *col );
unsigned int marked_bits = *col >> 16;
// check if the number of marked bits qualifies the col as marked
if ( !bitsBelowThreshold( marked_bits, marked_threshold ) ) {
return MARKED;
}
// check if the number of unkown bits qualifies the col as unknown
if ( !bitsBelowThreshold( unknown_bits, unknown_threshold ) ) {
return UNKNOWN;
}
return FREE;
}
221 unsigned int VoxelGrid::sizeX( )
{
return size_x_;
}
226 unsigned int VoxelGrid::sizeY( )
{
return size_y_;
}
231 unsigned int VoxelGrid::sizeZ( )
{
return size_z_;
}
236 void VoxelGrid::printVoxelGrid( )
{
for ( unsigned int z = 0; z < size_z_; z++ ) {
printf( "Layer z = %u:\n", z );
for ( unsigned int y = 0; y < size_y_; y++ ) {
for ( unsigned int x = 0; x < size_x_; x++ ) {
printf( ( getVoxel( x, y, z ) ) == nav2_voxel_grid::MARKED ? "#" : " " );
}
printf( "|\n" );
}
}
}
249 void VoxelGrid::printColumnGrid( )
{
printf( "Column view:\n" );
for ( unsigned int y = 0; y < size_y_; y++ ) {
for ( unsigned int x = 0; x < size_x_; x++ ) {
printf( ( getVoxelColumn( x, y, 16, 0 ) == nav2_voxel_grid::MARKED ) ? "#" : " " );
}
printf( "|\n" );
}
}
} // namespace nav2_voxel_grid
1 /*********************************************************************
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2021 Samsung Research Russia
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Alexey Merzlyakov
*********************************************************************/
#include <nav2_voxel_grid/voxel_grid.hpp>
#include <gtest/gtest.h>
39 class TestVoxel
{
public:
42 explicit TestVoxel( uint32_t * data, int sz_x, int sz_y )
: data_( data )
{
size_ = sz_x * sz_y;
}
47 inline void operator( )( unsigned int off, unsigned int val )
{
ASSERT_TRUE( off < size_ );
data_[off] = val;
}
52 inline unsigned int operator( )( unsigned int off )
{
return data_[off];
}
private:
58 uint32_t * data_;
unsigned int size_;
};
62 TEST( voxel_grid, bresenham3DBoundariesCheck )
{
const int sz_x = 60;
const int sz_y = 60;
const int sz_z = 2;
const unsigned int max_length = 60;
const unsigned int min_length = 6;
nav2_voxel_grid::VoxelGrid vg( sz_x, sz_y, sz_z );
TestVoxel tv( vg.getData( ), sz_x, sz_y );
// Initial point - some assymetrically standing point in order to cover most corner cases
const double x0 = 2.2;
const double y0 = 3.8;
const double z0 = 0.4;
// z-axis won't be domimant
const double z1 = 0.5;
// ( x1, y1 ) point will move
double x1, y1;
// Epsilon for outer boundaries of voxel grid array
const double epsilon = 0.02;
// Running on ( x, 0 ) edge
y1 = 0.0;
for ( int i = 0; i <= sz_x; i++ ) {
if ( i != sz_x ) {
x1 = i;
} else {
x1 = i - epsilon;
}
vg.raytraceLine( tv, x0, y0, z0, x1, y1, z1, max_length, min_length );
}
// Running on ( x, sz_y ) edge
y1 = sz_y - epsilon;
for ( int i = 0; i <= sz_x; i++ ) {
if ( i != sz_x ) {
x1 = i;
} else {
x1 = i - epsilon;
}
vg.raytraceLine( tv, x0, y0, z0, x1, y1, z1, max_length, min_length );
}
// Running on ( 0, y ) edge
x1 = 0.0;
for ( int j = 0; j <= sz_y; j++ ) {
if ( j != sz_y ) {
y1 = j;
} else {
y1 = j - epsilon;
}
vg.raytraceLine( tv, x0, y0, z0, x1, y1, z1, max_length, min_length );
}
// Running on ( sz_x, y ) edge
x1 = sz_x - epsilon;
for ( int j = 0; j <= sz_y; j++ ) {
if ( j != sz_y ) {
y1 = j;
} else {
y1 = j - epsilon;
}
vg.raytraceLine( tv, x0, y0, z0, x1, y1, z1, max_length, min_length );
}
}
129 TEST( voxel_grid, bresenham3DSamePoint )
{
const int sz_x = 60;
const int sz_y = 60;
const int sz_z = 2;
const unsigned int max_length = 60;
const unsigned int min_length = 0;
nav2_voxel_grid::VoxelGrid vg( sz_x, sz_y, sz_z );
TestVoxel tv( vg.getData( ), sz_x, sz_y );
// Initial point
const double x0 = 2.2;
const double y0 = 3.8;
const double z0 = 0.4;
unsigned int offset = static_cast<int>( y0 ) * sz_x + static_cast<int>( x0 );
unsigned int val_before = tv( offset );
// Same point to check
vg.raytraceLine( tv, x0, y0, z0, x0, y0, z0, max_length, min_length );
unsigned int val_after = tv( offset );
ASSERT_FALSE( val_before == val_after );
}
152 int main( int argc, char ** argv )
{
testing::InitGoogleTest( &argc, argv );
return RUN_ALL_TESTS( );
}
1 /*********************************************************************
*
* Software License Agreement ( BSD License )
*
* Copyright ( c ) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES ( INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION ) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE ) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#include <nav2_voxel_grid/voxel_grid.hpp>
#include <gtest/gtest.h>
40 TEST( voxel_grid, basicMarkingAndClearing ) {
int size_x = 50, size_y = 10, size_z = 16;
nav2_voxel_grid::VoxelGrid vg( size_x, size_y, size_z );
// Put a "tabletop" into the scene. A flat rectangle of set voxels at z = 12.
int table_z = 12;
int table_x_min = 5, table_x_max = 15;
int table_y_min = 0, table_y_max = 3;
for ( int x = table_x_min; x <= table_x_max; x++ ) {
vg.markVoxelLine( x, table_y_min, table_z, x, table_y_max, table_z );
}
for ( int i = table_x_min; i <= table_x_max; ++i ) {
for ( int j = table_y_min; j <= table_y_max; ++j ) {
// check that each cell of the table is marked
ASSERT_EQ( nav2_voxel_grid::MARKED, vg.getVoxel( i, j, table_z ) );
}
}
int mark_count = 0;
unsigned int unknown_count = 0;
// go through each cell in the voxel grid and make sure that only 44 are filled in
for ( unsigned int i = 0; i < vg.sizeX( ); ++i ) {
for ( unsigned int j = 0; j < vg.sizeY( ); ++j ) {
for ( unsigned int k = 0; k < vg.sizeZ( ); ++k ) {
if ( vg.getVoxel( i, j, k ) == nav2_voxel_grid::MARKED ) {
mark_count++;
} else if ( vg.getVoxel( i, j, k ) == nav2_voxel_grid::UNKNOWN ) {
unknown_count++;
}
}
}
}
ASSERT_EQ( mark_count, 44 );
// the rest of the cells should be unknown
ASSERT_EQ( unknown_count, vg.sizeX( ) * vg.sizeY( ) * vg.sizeZ( ) - 44 );
// now, let's clear one of the rows of the table
vg.clearVoxelLine( table_x_min, table_y_min, table_z, table_x_max, table_y_min, table_z );
mark_count = 0;
unknown_count = 0;
int free_count = 0;
// go through each cell in the voxel grid and make sure that only 33 are now filled in
for ( unsigned int i = 0; i < vg.sizeX( ); ++i ) {
for ( unsigned int j = 0; j < vg.sizeY( ); ++j ) {
for ( unsigned int k = 0; k < vg.sizeZ( ); ++k ) {
if ( vg.getVoxel( i, j, k ) == nav2_voxel_grid::MARKED ) {
mark_count++;
} else if ( vg.getVoxel( i, j, k ) == nav2_voxel_grid::FREE ) {
free_count++;
} else if ( vg.getVoxel( i, j, k ) == nav2_voxel_grid::UNKNOWN ) {
unknown_count++;
}
}
}
}
// we've switched 11 cells from marked to free
ASSERT_EQ( mark_count, 33 );
// we've just explicitly seen through 11 cells
ASSERT_EQ( free_count, 11 );
// the rest of the cells should still be unknown
ASSERT_EQ( unknown_count, vg.sizeX( ) * vg.sizeY( ) * vg.sizeZ( ) - 44 );
// now let's put in a vertical column manually to test markVoxel
for ( unsigned int i = 0; i < vg.sizeZ( ); ++i ) {
vg.markVoxel( 0, 0, i );
ASSERT_EQ( vg.getVoxel( 0, 0, i ), nav2_voxel_grid::MARKED );
}
vg.printColumnGrid( );
vg.printVoxelGrid( );
// now, let's clear that line of voxels and make sure that they clear out OK
vg.clearVoxelLine( 0, 0, 0, 0, 0, vg.sizeZ( ) - 1 );
for ( unsigned int i = 0; i < vg.sizeZ( ); ++i ) {
ASSERT_EQ( vg.getVoxel( 0, 0, i ), nav2_voxel_grid::FREE );
}
mark_count = 0;
// Visualize the output
/*
v->printVoxelGrid( );
v->printColumnGrid( );
printf( "CostMap:\n===========\n" );
for( int y = 0; y < size_y; y++ ){
for( int x = 0; x < size_x; x++ ){
printf( ( costMap[y * size_x + x] > 0 ? "#" : " " ) );
}printf( "|\n" );
}
*/
}
140 TEST( voxel_grid, InvalidSize ) {
int size_x = 50, size_y = 10, size_z = 17;
int test_z = 16;
nav2_voxel_grid::VoxelGrid vg( size_x, size_y, size_z );
vg.resize( size_x, size_y, test_z );
vg.resize( size_x, size_y, size_z );
EXPECT_TRUE( vg.getVoxelColumn( 51, 10, 0, 0 ) == nav2_voxel_grid::VoxelStatus::UNKNOWN );
EXPECT_TRUE( vg.getVoxelColumn( 50, 11, 0, 0 ) == nav2_voxel_grid::VoxelStatus::UNKNOWN );
}
150 TEST( voxel_grid, MarkAndClear ) {
int size_x = 10, size_y = 10, size_z = 10;
nav2_voxel_grid::VoxelGrid vg( size_x, size_y, size_z );
vg.markVoxelInMap( 5, 5, 5, 0 );
EXPECT_EQ( vg.getVoxel( 5, 5, 5 ), nav2_voxel_grid::MARKED );
vg.clearVoxelColumn( 55 );
EXPECT_EQ( vg.getVoxel( 5, 5, 5 ), nav2_voxel_grid::FREE );
}
159 TEST( voxel_grid, clearVoxelLineInMap ) {
int size_x = 10, size_y = 10, size_z = 10;
nav2_voxel_grid::VoxelGrid vg( size_x, size_y, size_z );
vg.markVoxelInMap( 0, 0, 5, 0 );
EXPECT_EQ( vg.getVoxel( 0, 0, 5 ), nav2_voxel_grid::MARKED );
unsigned char * map_2d = new unsigned char[100];
map_2d[0] = 254;
vg.clearVoxelLineInMap( 0, 0, 0, 0, 0, 9, map_2d, 16, 0 );
EXPECT_EQ( map_2d[0], 0 );
vg.markVoxelInMap( 0, 0, 5, 0 );
vg.clearVoxelLineInMap( 0, 0, 0, 0, 0, 9, nullptr, 16, 0 );
EXPECT_EQ( vg.getVoxel( 0, 0, 5 ), nav2_voxel_grid::FREE );
// Testing for min range for raytrace clearing
vg.markVoxelInMap( 0, 0, 5, 0 );
vg.markVoxelInMap( 0, 0, 7, 0 );
vg.clearVoxelLineInMap(
0, 0, 0, 0, 0, 9, nullptr, 16, 0, ( unsigned char )'\000',
( unsigned char )'\377', UINT_MAX, 6 );
EXPECT_EQ( vg.getVoxel( 0, 0, 5 ), nav2_voxel_grid::MARKED );
EXPECT_EQ( vg.getVoxel( 0, 0, 7 ), nav2_voxel_grid::FREE );
delete[] map_2d;
}
188 TEST( voxel_grid, GetVoxelData ) {
uint32_t * data = new uint32_t[9];
data[4] = 255;
data[0] = 0;
EXPECT_EQ(
nav2_voxel_grid::VoxelGrid::getVoxel( 1, 1, 1, 3, 3, 3, data ), nav2_voxel_grid::UNKNOWN );
EXPECT_EQ(
nav2_voxel_grid::VoxelGrid::getVoxel( 0, 0, 0, 3, 3, 3, data ), nav2_voxel_grid::FREE );
delete[] data;
}
200 int main( int argc, char ** argv )
{
testing::InitGoogleTest( &argc, argv );
return RUN_ALL_TESTS( );
}
// Copyright ( c ) 2020 Samsung Research America
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_WAYPOINT_FOLLOWER__PLUGINS__INPUT_AT_WAYPOINT_HPP_
#define NAV2_WAYPOINT_FOLLOWER__PLUGINS__INPUT_AT_WAYPOINT_HPP_
#pragma once
#include <string>
#include <mutex>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/empty.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "nav2_core/waypoint_task_executor.hpp"
namespace nav2_waypoint_follower
{
/**
* @brief Simple plugin based on WaypointTaskExecutor, lets robot to wait for a
* user input at waypoint arrival.
*/
34 class InputAtWaypoint : public nav2_core::WaypointTaskExecutor
{
public:
/**
* @brief Construct a new Input At Waypoint Arrival object
*
*/
41 InputAtWaypoint( );
/**
* @brief Destroy the Input At Waypoint Arrival object
*
*/
47 ~InputAtWaypoint( );
/**
* @brief declares and loads parameters used
* @param parent parent node
* @param plugin_name name of plugin
*/
54 void initialize(
55 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
56 const std::string & plugin_name );
/**
* @brief Processor
* @param curr_pose current pose of the robot
* @param curr_waypoint_index current waypoint, that robot just arrived
* @return if task execution failed
*/
64 bool processAtWaypoint(
65 const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index );
protected:
/**
* @brief Processor callback
* @param msg Empty message
*/
72 void Cb( const std_msgs::msg::Empty::SharedPtr msg );
74 bool input_received_;
75 bool is_enabled_;
76 rclcpp::Duration timeout_;
rclcpp::Logger logger_{rclcpp::get_logger( "nav2_waypoint_follower" )};
rclcpp::Clock::SharedPtr clock_;
std::mutex mutex_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr subscription_;
};
} // namespace nav2_waypoint_follower
#endif // NAV2_WAYPOINT_FOLLOWER__PLUGINS__INPUT_AT_WAYPOINT_HPP_
// Copyright ( c ) 2020 Fetullah Atas
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_WAYPOINT_FOLLOWER__PLUGINS__PHOTO_AT_WAYPOINT_HPP_
#define NAV2_WAYPOINT_FOLLOWER__PLUGINS__PHOTO_AT_WAYPOINT_HPP_
/**
* While C++17 isn't the project standard. We have to force LLVM/CLang
* to ignore deprecated declarations
*/
#define _LIBCPP_NO_EXPERIMENTAL_DEPRECATION_WARNING_FILESYSTEM
#include <experimental/filesystem>
#include <mutex>
#include <string>
#include <exception>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_components/register_node_macro.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "nav2_core/waypoint_task_executor.hpp"
#include "opencv4/opencv2/core.hpp"
#include "opencv4/opencv2/opencv.hpp"
#include "cv_bridge/cv_bridge.h"
#include "image_transport/image_transport.hpp"
namespace nav2_waypoint_follower
{
44 class PhotoAtWaypoint : public nav2_core::WaypointTaskExecutor
{
public:
/**
* @brief Construct a new Photo At Waypoint object
*
*/
51 PhotoAtWaypoint( );
/**
* @brief Destroy the Photo At Waypoint object
*
*/
57 ~PhotoAtWaypoint( );
/**
* @brief declares and loads parameters used
*
* @param parent parent node that plugin will be created within
* @param plugin_name should be provided in nav2_params.yaml==> waypoint_follower
*/
65 void initialize(
66 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
67 const std::string & plugin_name );
/**
* @brief Override this to define the body of your task that you would like to execute once the robot arrived to waypoint
*
* @param curr_pose current pose of the robot
* @param curr_waypoint_index current waypoint, that robot just arrived
* @return true if task execution was successful
* @return false if task execution failed
*/
78 bool processAtWaypoint(
79 const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index );
/**
* @brief
*
* @param msg
*/
86 void imageCallback( const sensor_msgs::msg::Image::SharedPtr msg );
/**
* @brief given a shared pointer to sensor::msg::Image type, make a deep copy to inputted cv Mat
*
* @param msg
* @param mat
*/
94 static void deepCopyMsg2Mat( const sensor_msgs::msg::Image::SharedPtr & msg, cv::Mat & mat );
protected:
// to ensure safety when accessing global var curr_frame_
98 std::mutex global_mutex_;
// the taken photos will be saved under this directory
100 std::experimental::filesystem::path save_dir_;
// .png ? .jpg ? or some other well known format
102 std::string image_format_;
// the topic to subscribe in order capture a frame
104 std::string image_topic_;
// whether plugin is enabled
106 bool is_enabled_;
// current frame;
108 sensor_msgs::msg::Image::SharedPtr curr_frame_msg_;
// global logger
rclcpp::Logger logger_{rclcpp::get_logger( "nav2_waypoint_follower" )};
// ros susbcriber to get camera image
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr camera_image_subscriber_;
};
} // namespace nav2_waypoint_follower
#endif // NAV2_WAYPOINT_FOLLOWER__PLUGINS__PHOTO_AT_WAYPOINT_HPP_
// Copyright ( c ) 2020 Fetullah Atas
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_WAYPOINT_FOLLOWER__PLUGINS__WAIT_AT_WAYPOINT_HPP_
#define NAV2_WAYPOINT_FOLLOWER__PLUGINS__WAIT_AT_WAYPOINT_HPP_
#pragma once
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "nav2_core/waypoint_task_executor.hpp"
namespace nav2_waypoint_follower
{
/**
* @brief Simple plugin based on WaypointTaskExecutor, lets robot to sleep for a
* specified amount of time at waypoint arrival. You can reference this class to define
* your own task and rewrite the body for it.
*
*/
34 class WaitAtWaypoint : public nav2_core::WaypointTaskExecutor
{
public:
/**
* @brief Construct a new Wait At Waypoint Arrival object
*
*/
41 WaitAtWaypoint( );
/**
* @brief Destroy the Wait At Waypoint Arrival object
*
*/
47 ~WaitAtWaypoint( );
/**
* @brief declares and loads parameters used ( waypoint_pause_duration_ )
*
* @param parent parent node that plugin will be created withing( waypoint_follower in this case )
* @param plugin_name
*/
55 void initialize(
56 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
57 const std::string & plugin_name );
/**
* @brief Override this to define the body of your task that you would like to execute once the robot arrived to waypoint
*
* @param curr_pose current pose of the robot
* @param curr_waypoint_index current waypoint, that robot just arrived
* @return true if task execution was successful
* @return false if task execution failed
*/
68 bool processAtWaypoint(
69 const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index );
protected:
// the robot will sleep waypoint_pause_duration_ milliseconds
int waypoint_pause_duration_;
74 bool is_enabled_;
rclcpp::Logger logger_{rclcpp::get_logger( "nav2_waypoint_follower" )};
};
} // namespace nav2_waypoint_follower
#endif // NAV2_WAYPOINT_FOLLOWER__PLUGINS__WAIT_AT_WAYPOINT_HPP_
1 // Copyright ( c ) 2019 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef NAV2_WAYPOINT_FOLLOWER__WAYPOINT_FOLLOWER_HPP_
#define NAV2_WAYPOINT_FOLLOWER__WAYPOINT_FOLLOWER_HPP_
#include <memory>
#include <string>
#include <vector>
#include <mutex>
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"
#include "nav2_msgs/action/follow_waypoints.hpp"
#include "nav_msgs/msg/path.hpp"
#include "nav2_util/simple_action_server.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_core/waypoint_task_executor.hpp"
#include "pluginlib/class_loader.hpp"
#include "pluginlib/class_list_macros.hpp"
namespace nav2_waypoint_follower
{
38 enum class ActionStatus
{
UNKNOWN = 0,
PROCESSING = 1,
FAILED = 2,
SUCCEEDED = 3
};
/**
* @class nav2_waypoint_follower::WaypointFollower
* @brief An action server that uses behavior tree for navigating a robot to its
* goal position.
*/
51 class WaypointFollower : public nav2_util::LifecycleNode
{
public:
using ActionT = nav2_msgs::action::FollowWaypoints;
using ClientT = nav2_msgs::action::NavigateToPose;
using ActionServer = nav2_util::SimpleActionServer<ActionT>;
using ActionClient = rclcpp_action::Client<ClientT>;
/**
* @brief A constructor for nav2_waypoint_follower::WaypointFollower class
* @param options Additional options to control creation of the node.
*/
explicit WaypointFollower( const rclcpp::NodeOptions & options = rclcpp::NodeOptions( ) );
/**
* @brief A destructor for nav2_waypoint_follower::WaypointFollower class
*/
~WaypointFollower( );
protected:
/**
* @brief Configures member variables
*
* Initializes action server for "follow_waypoints"
* @param state Reference to LifeCycle node state
* @return SUCCESS or FAILURE
*/
nav2_util::CallbackReturn on_configure( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Activates action server
* @param state Reference to LifeCycle node state
* @return SUCCESS or FAILURE
*/
nav2_util::CallbackReturn on_activate( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Deactivates action server
* @param state Reference to LifeCycle node state
* @return SUCCESS or FAILURE
*/
nav2_util::CallbackReturn on_deactivate( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Resets member variables
* @param state Reference to LifeCycle node state
* @return SUCCESS or FAILURE
*/
nav2_util::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Called when in shutdown state
* @param state Reference to LifeCycle node state
* @return SUCCESS or FAILURE
*/
nav2_util::CallbackReturn on_shutdown( const rclcpp_lifecycle::State & state ) override;
/**
* @brief Action server callbacks
*/
void followWaypoints( );
/**
* @brief Action client result callback
* @param result Result of action server updated asynchronously
*/
void resultCallback( const rclcpp_action::ClientGoalHandle<ClientT>::WrappedResult & result );
/**
* @brief Action client goal response callback
* @param goal Response of action server updated asynchronously
*/
void goalResponseCallback( const rclcpp_action::ClientGoalHandle<ClientT>::SharedPtr & goal );
/**
* @brief Callback executed when a parameter change is detected
* @param event ParameterEvent message
*/
rcl_interfaces::msg::SetParametersResult
dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters );
// Dynamic parameters handler
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
// Our action server
std::unique_ptr<ActionServer> action_server_;
ActionClient::SharedPtr nav_to_pose_client_;
rclcpp::CallbackGroup::SharedPtr callback_group_;
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
std::shared_future<rclcpp_action::ClientGoalHandle<ClientT>::SharedPtr> future_goal_handle_;
bool stop_on_failure_;
ActionStatus current_goal_status_;
int loop_rate_;
std::vector<int> failed_ids_;
// Task Execution At Waypoint Plugin
pluginlib::ClassLoader<nav2_core::WaypointTaskExecutor>
waypoint_task_executor_loader_;
pluginlib::UniquePtr<nav2_core::WaypointTaskExecutor>
waypoint_task_executor_;
std::string waypoint_task_executor_id_;
std::string waypoint_task_executor_type_;
};
} // namespace nav2_waypoint_follower
#endif // NAV2_WAYPOINT_FOLLOWER__WAYPOINT_FOLLOWER_HPP_
1 // Copyright ( c ) 2020 Samsung Research America
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "nav2_waypoint_follower/plugins/input_at_waypoint.hpp"
#include <string>
#include <exception>
#include "pluginlib/class_list_macros.hpp"
#include "nav2_util/node_utils.hpp"
namespace nav2_waypoint_follower
{
using std::placeholders::_1;
28 InputAtWaypoint::InputAtWaypoint( )
: input_received_( false ),
is_enabled_( true ),
timeout_( 10.0, 0.0 )
{
}
35 InputAtWaypoint::~InputAtWaypoint( )
{
}
39 void InputAtWaypoint::initialize(
40 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
41 const std::string & plugin_name )
{
auto node = parent.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node in input at waypoint plugin!"};
}
logger_ = node->get_logger( );
clock_ = node->get_clock( );
double timeout;
std::string input_topic;
nav2_util::declare_parameter_if_not_declared(
node, plugin_name + ".timeout",
rclcpp::ParameterValue( 10.0 ) );
nav2_util::declare_parameter_if_not_declared(
node, plugin_name + ".enabled",
rclcpp::ParameterValue( true ) );
nav2_util::declare_parameter_if_not_declared(
node, plugin_name + ".input_topic",
rclcpp::ParameterValue( "input_at_waypoint/input" ) );
node->get_parameter( plugin_name + ".timeout", timeout );
node->get_parameter( plugin_name + ".enabled", is_enabled_ );
node->get_parameter( plugin_name + ".input_topic", input_topic );
timeout_ = rclcpp::Duration( timeout, 0.0 );
RCLCPP_INFO(
logger_, "InputAtWaypoint: Subscribing to input topic %s.", input_topic.c_str( ) );
subscription_ = node->create_subscription<std_msgs::msg::Empty>(
input_topic, 1, std::bind( &InputAtWaypoint::Cb, this, _1 ) );
}
75 void InputAtWaypoint::Cb( const std_msgs::msg::Empty::SharedPtr /*msg*/ )
{
std::lock_guard<std::mutex> lock( mutex_ );
input_received_ = true;
}
81 bool InputAtWaypoint::processAtWaypoint(
82 const geometry_msgs::msg::PoseStamped & /*curr_pose*/,
const int & curr_waypoint_index )
{
if ( !is_enabled_ ) {
return true;
}
input_received_ = false;
rclcpp::Time start = clock_->now( );
rclcpp::Rate r( 50 );
bool input_received = false;
while ( clock_->now( ) - start < timeout_ ) {
{
std::lock_guard<std::mutex> lock( mutex_ );
input_received = input_received_;
}
if ( input_received ) {
return true;
}
r.sleep( );
}
RCLCPP_WARN(
logger_, "Unable to get external input at wp %i. Moving on.", curr_waypoint_index );
return false;
}
} // namespace nav2_waypoint_follower
114 PLUGINLIB_EXPORT_CLASS(
nav2_waypoint_follower::InputAtWaypoint,
nav2_core::WaypointTaskExecutor )
1 // Copyright ( c ) 2020 Fetullah Atas
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "nav2_waypoint_follower/plugins/photo_at_waypoint.hpp"
#include <string>
#include <memory>
#include "pluginlib/class_list_macros.hpp"
#include "nav2_util/node_utils.hpp"
namespace nav2_waypoint_follower
{
26 PhotoAtWaypoint::PhotoAtWaypoint( )
{
}
30 PhotoAtWaypoint::~PhotoAtWaypoint( )
{
}
34 void PhotoAtWaypoint::initialize(
35 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
36 const std::string & plugin_name )
{
auto node = parent.lock( );
curr_frame_msg_ = std::make_shared<sensor_msgs::msg::Image>( );
nav2_util::declare_parameter_if_not_declared(
node, plugin_name + ".enabled",
rclcpp::ParameterValue( true ) );
nav2_util::declare_parameter_if_not_declared(
node, plugin_name + ".image_topic",
rclcpp::ParameterValue( "/camera/color/image_raw" ) );
nav2_util::declare_parameter_if_not_declared(
node, plugin_name + ".save_dir",
rclcpp::ParameterValue( "/tmp/waypoint_images" ) );
nav2_util::declare_parameter_if_not_declared(
node, plugin_name + ".image_format",
rclcpp::ParameterValue( "png" ) );
std::string save_dir_as_string;
node->get_parameter( plugin_name + ".enabled", is_enabled_ );
node->get_parameter( plugin_name + ".image_topic", image_topic_ );
node->get_parameter( plugin_name + ".save_dir", save_dir_as_string );
node->get_parameter( plugin_name + ".image_format", image_format_ );
// get inputted save directory and make sure it exists, if not log and create it
save_dir_ = save_dir_as_string;
try {
if ( !std::experimental::filesystem::exists( save_dir_ ) ) {
RCLCPP_WARN(
logger_,
"Provided save directory for photo at waypoint plugin does not exist, "
"provided directory is: %s, the directory will be created automatically.",
save_dir_.c_str( )
);
if ( !std::experimental::filesystem::create_directory( save_dir_ ) ) {
RCLCPP_ERROR(
logger_,
"Failed to create directory!: %s required by photo at waypoint plugin, "
"exiting the plugin with failure!",
save_dir_.c_str( )
);
is_enabled_ = false;
}
}
} catch ( const std::exception & e ) {
RCLCPP_ERROR(
logger_, "Exception ( %s ) thrown while attempting to create image capture directory."
" This task executor is being disabled as it cannot save images.", e.what( ) );
is_enabled_ = false;
}
if ( !is_enabled_ ) {
RCLCPP_INFO(
logger_, "Photo at waypoint plugin is disabled." );
} else {
RCLCPP_INFO(
logger_, "Initializing photo at waypoint plugin, subscribing to camera topic named; %s",
image_topic_.c_str( ) );
camera_image_subscriber_ = node->create_subscription<sensor_msgs::msg::Image>(
image_topic_, rclcpp::SystemDefaultsQoS( ),
std::bind( &PhotoAtWaypoint::imageCallback, this, std::placeholders::_1 ) );
}
}
101 bool PhotoAtWaypoint::processAtWaypoint(
102 const geometry_msgs::msg::PoseStamped & curr_pose, const int & curr_waypoint_index )
{
if ( !is_enabled_ ) {
RCLCPP_WARN(
logger_,
"Photo at waypoint plugin is disabled. Not performing anything"
);
return true;
}
try {
// construct the full path to image filename
std::experimental::filesystem::path file_name = std::to_string(
curr_waypoint_index ) + "_" +
std::to_string( curr_pose.header.stamp.sec ) + "." + image_format_;
std::experimental::filesystem::path full_path_image_path = save_dir_ / file_name;
// save the taken photo at this waypoint to given directory
std::lock_guard<std::mutex> guard( global_mutex_ );
cv::Mat curr_frame_mat;
deepCopyMsg2Mat( curr_frame_msg_, curr_frame_mat );
cv::imwrite( full_path_image_path.c_str( ), curr_frame_mat );
RCLCPP_INFO(
logger_,
"Photo has been taken sucessfully at waypoint %i", curr_waypoint_index );
} catch ( const std::exception & e ) {
RCLCPP_ERROR(
logger_,
"Couldn't take photo at waypoint %i! Caught exception: %s \n"
"Make sure that the image topic named: %s is valid and active!",
curr_waypoint_index,
e.what( ), image_topic_.c_str( ) );
return false;
}
return true;
}
138 void PhotoAtWaypoint::imageCallback( const sensor_msgs::msg::Image::SharedPtr msg )
{
std::lock_guard<std::mutex> guard( global_mutex_ );
curr_frame_msg_ = msg;
}
144 void PhotoAtWaypoint::deepCopyMsg2Mat(
145 const sensor_msgs::msg::Image::SharedPtr & msg,
146 cv::Mat & mat )
{
cv_bridge::CvImageConstPtr cv_bridge_ptr = cv_bridge::toCvShare( msg, msg->encoding );
cv::Mat frame = cv_bridge_ptr->image;
if ( msg->encoding == "rgb8" ) {
cv::cvtColor( frame, frame, cv::COLOR_RGB2BGR );
}
frame.copyTo( mat );
}
} // namespace nav2_waypoint_follower
157 PLUGINLIB_EXPORT_CLASS(
nav2_waypoint_follower::PhotoAtWaypoint,
nav2_core::WaypointTaskExecutor )
1 // Copyright ( c ) 2020 Fetullah Atas
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "nav2_waypoint_follower/plugins/wait_at_waypoint.hpp"
#include <string>
#include <exception>
#include "pluginlib/class_list_macros.hpp"
#include "nav2_util/node_utils.hpp"
namespace nav2_waypoint_follower
{
26 WaitAtWaypoint::WaitAtWaypoint( )
: waypoint_pause_duration_( 0 ),
is_enabled_( true )
{
}
32 WaitAtWaypoint::~WaitAtWaypoint( )
{
}
36 void WaitAtWaypoint::initialize(
37 const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
38 const std::string & plugin_name )
{
auto node = parent.lock( );
if ( !node ) {
throw std::runtime_error{"Failed to lock node in wait at waypoint plugin!"};
}
logger_ = node->get_logger( );
nav2_util::declare_parameter_if_not_declared(
node,
plugin_name + ".waypoint_pause_duration",
rclcpp::ParameterValue( 0 ) );
nav2_util::declare_parameter_if_not_declared(
node,
plugin_name + ".enabled",
rclcpp::ParameterValue( true ) );
node->get_parameter(
plugin_name + ".waypoint_pause_duration",
waypoint_pause_duration_ );
node->get_parameter(
plugin_name + ".enabled",
is_enabled_ );
if ( waypoint_pause_duration_ == 0 ) {
is_enabled_ = false;
RCLCPP_INFO(
logger_,
"Waypoint pause duration is set to zero, disabling task executor plugin." );
} else if ( !is_enabled_ ) {
RCLCPP_INFO(
logger_, "Waypoint task executor plugin is disabled." );
}
}
70 bool WaitAtWaypoint::processAtWaypoint(
71 const geometry_msgs::msg::PoseStamped & /*curr_pose*/, const int & curr_waypoint_index )
{
if ( !is_enabled_ ) {
return true;
}
RCLCPP_INFO(
logger_, "Arrived at %i'th waypoint, sleeping for %i milliseconds",
curr_waypoint_index,
waypoint_pause_duration_ );
rclcpp::sleep_for( std::chrono::milliseconds( waypoint_pause_duration_ ) );
return true;
}
} // namespace nav2_waypoint_follower
84 PLUGINLIB_EXPORT_CLASS(
nav2_waypoint_follower::WaitAtWaypoint,
nav2_core::WaypointTaskExecutor )
1 // Copyright ( c ) 2019 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include "nav2_waypoint_follower/waypoint_follower.hpp"
#include "rclcpp/rclcpp.hpp"
20 int main( int argc, char ** argv )
{
rclcpp::init( argc, argv );
auto node = std::make_shared<nav2_waypoint_follower::WaypointFollower>( );
rclcpp::spin( node->get_node_base_interface( ) );
rclcpp::shutdown( );
return 0;
}
1 // Copyright ( c ) 2019 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "nav2_waypoint_follower/waypoint_follower.hpp"
#include <fstream>
#include <memory>
#include <streambuf>
#include <string>
#include <utility>
#include <vector>
namespace nav2_waypoint_follower
{
using rcl_interfaces::msg::ParameterType;
using std::placeholders::_1;
30 WaypointFollower::WaypointFollower( const rclcpp::NodeOptions & options )
: nav2_util::LifecycleNode( "waypoint_follower", "", options ),
waypoint_task_executor_loader_( "nav2_waypoint_follower",
"nav2_core::WaypointTaskExecutor" )
{
RCLCPP_INFO( get_logger( ), "Creating" );
declare_parameter( "stop_on_failure", true );
declare_parameter( "loop_rate", 20 );
nav2_util::declare_parameter_if_not_declared(
this, std::string( "waypoint_task_executor_plugin" ),
rclcpp::ParameterValue( std::string( "wait_at_waypoint" ) ) );
nav2_util::declare_parameter_if_not_declared(
this, std::string( "wait_at_waypoint.plugin" ),
rclcpp::ParameterValue( std::string( "nav2_waypoint_follower::WaitAtWaypoint" ) ) );
}
47 WaypointFollower::~WaypointFollower( )
{
}
nav2_util::CallbackReturn
52 WaypointFollower::on_configure( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Configuring" );
auto node = shared_from_this( );
stop_on_failure_ = get_parameter( "stop_on_failure" ).as_bool( );
loop_rate_ = get_parameter( "loop_rate" ).as_int( );
waypoint_task_executor_id_ = get_parameter( "waypoint_task_executor_plugin" ).as_string( );
callback_group_ = create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive,
false );
callback_group_executor_.add_callback_group( callback_group_, get_node_base_interface( ) );
nav_to_pose_client_ = rclcpp_action::create_client<ClientT>(
get_node_base_interface( ),
get_node_graph_interface( ),
get_node_logging_interface( ),
get_node_waitables_interface( ),
"navigate_to_pose", callback_group_ );
action_server_ = std::make_unique<ActionServer>(
get_node_base_interface( ),
get_node_clock_interface( ),
get_node_logging_interface( ),
get_node_waitables_interface( ),
"follow_waypoints", std::bind( &WaypointFollower::followWaypoints, this ) );
try {
waypoint_task_executor_type_ = nav2_util::get_plugin_type_param(
this,
waypoint_task_executor_id_ );
waypoint_task_executor_ = waypoint_task_executor_loader_.createUniqueInstance(
waypoint_task_executor_type_ );
RCLCPP_INFO(
get_logger( ), "Created waypoint_task_executor : %s of type %s",
waypoint_task_executor_id_.c_str( ), waypoint_task_executor_type_.c_str( ) );
waypoint_task_executor_->initialize( node, waypoint_task_executor_id_ );
} catch ( const pluginlib::PluginlibException & ex ) {
RCLCPP_FATAL(
get_logger( ),
"Failed to create waypoint_task_executor. Exception: %s", ex.what( ) );
}
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
101 WaypointFollower::on_activate( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Activating" );
action_server_->activate( );
auto node = shared_from_this( );
// Add callback for dynamic parameters
dyn_params_handler_ = node->add_on_set_parameters_callback(
std::bind( &WaypointFollower::dynamicParametersCallback, this, _1 ) );
// create bond connection
createBond( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
119 WaypointFollower::on_deactivate( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Deactivating" );
action_server_->deactivate( );
dyn_params_handler_.reset( );
// destroy bond connection
destroyBond( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
133 WaypointFollower::on_cleanup( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Cleaning up" );
action_server_.reset( );
nav_to_pose_client_.reset( );
return nav2_util::CallbackReturn::SUCCESS;
}
nav2_util::CallbackReturn
144 WaypointFollower::on_shutdown( const rclcpp_lifecycle::State & /*state*/ )
{
RCLCPP_INFO( get_logger( ), "Shutting down" );
return nav2_util::CallbackReturn::SUCCESS;
}
void
151 WaypointFollower::followWaypoints( )
{
auto goal = action_server_->get_current_goal( );
auto feedback = std::make_shared<ActionT::Feedback>( );
auto result = std::make_shared<ActionT::Result>( );
// Check if request is valid
if ( !action_server_ || !action_server_->is_server_active( ) ) {
RCLCPP_DEBUG( get_logger( ), "Action server inactive. Stopping." );
return;
}
RCLCPP_INFO(
get_logger( ), "Received follow waypoint request with %i waypoints.",
static_cast<int>( goal->poses.size( ) ) );
if ( goal->poses.size( ) == 0 ) {
action_server_->succeeded_current( result );
return;
}
rclcpp::WallRate r( loop_rate_ );
uint32_t goal_index = 0;
bool new_goal = true;
while ( rclcpp::ok( ) ) {
// Check if asked to stop processing action
if ( action_server_->is_cancel_requested( ) ) {
auto cancel_future = nav_to_pose_client_->async_cancel_all_goals( );
callback_group_executor_.spin_until_future_complete( cancel_future );
// for result callback processing
callback_group_executor_.spin_some( );
action_server_->terminate_all( );
return;
}
// Check if asked to process another action
if ( action_server_->is_preempt_requested( ) ) {
RCLCPP_INFO( get_logger( ), "Preempting the goal pose." );
goal = action_server_->accept_pending_goal( );
goal_index = 0;
new_goal = true;
}
// Check if we need to send a new goal
if ( new_goal ) {
new_goal = false;
ClientT::Goal client_goal;
client_goal.pose = goal->poses[goal_index];
auto send_goal_options = rclcpp_action::Client<ClientT>::SendGoalOptions( );
send_goal_options.result_callback =
std::bind( &WaypointFollower::resultCallback, this, std::placeholders::_1 );
send_goal_options.goal_response_callback =
std::bind( &WaypointFollower::goalResponseCallback, this, std::placeholders::_1 );
future_goal_handle_ =
nav_to_pose_client_->async_send_goal( client_goal, send_goal_options );
current_goal_status_ = ActionStatus::PROCESSING;
}
feedback->current_waypoint = goal_index;
action_server_->publish_feedback( feedback );
if ( current_goal_status_ == ActionStatus::FAILED ) {
failed_ids_.push_back( goal_index );
if ( stop_on_failure_ ) {
RCLCPP_WARN(
get_logger( ), "Failed to process waypoint %i in waypoint "
"list and stop on failure is enabled."
" Terminating action.", goal_index );
result->missed_waypoints = failed_ids_;
action_server_->terminate_current( result );
failed_ids_.clear( );
return;
} else {
RCLCPP_INFO(
get_logger( ), "Failed to process waypoint %i, "
" moving to next.", goal_index );
}
} else if ( current_goal_status_ == ActionStatus::SUCCEEDED ) {
RCLCPP_INFO(
get_logger( ), "Succeeded processing waypoint %i, processing waypoint task execution",
goal_index );
bool is_task_executed = waypoint_task_executor_->processAtWaypoint(
goal->poses[goal_index], goal_index );
RCLCPP_INFO(
get_logger( ), "Task execution at waypoint %i %s", goal_index,
is_task_executed ? "succeeded" : "failed!" );
// if task execution was failed and stop_on_failure_ is on , terminate action
if ( !is_task_executed && stop_on_failure_ ) {
failed_ids_.push_back( goal_index );
RCLCPP_WARN(
get_logger( ), "Failed to execute task at waypoint %i "
" stop on failure is enabled."
" Terminating action.", goal_index );
result->missed_waypoints = failed_ids_;
action_server_->terminate_current( result );
failed_ids_.clear( );
return;
} else {
RCLCPP_INFO(
get_logger( ), "Handled task execution on waypoint %i, "
" moving to next.", goal_index );
}
}
if ( current_goal_status_ != ActionStatus::PROCESSING &&
current_goal_status_ != ActionStatus::UNKNOWN )
{
// Update server state
goal_index++;
new_goal = true;
if ( goal_index >= goal->poses.size( ) ) {
RCLCPP_INFO(
get_logger( ), "Completed all %zu waypoints requested.",
goal->poses.size( ) );
result->missed_waypoints = failed_ids_;
action_server_->succeeded_current( result );
failed_ids_.clear( );
return;
}
} else {
RCLCPP_INFO_EXPRESSION(
get_logger( ),
( static_cast<int>( now( ).seconds( ) ) % 30 == 0 ),
"Processing waypoint %i...", goal_index );
}
callback_group_executor_.spin_some( );
r.sleep( );
}
}
void
286 WaypointFollower::resultCallback(
287 const rclcpp_action::ClientGoalHandle<ClientT>::WrappedResult & result )
{
switch ( result.code ) {
case rclcpp_action::ResultCode::SUCCEEDED:
current_goal_status_ = ActionStatus::SUCCEEDED;
return;
case rclcpp_action::ResultCode::ABORTED:
current_goal_status_ = ActionStatus::FAILED;
return;
case rclcpp_action::ResultCode::CANCELED:
current_goal_status_ = ActionStatus::FAILED;
return;
default:
current_goal_status_ = ActionStatus::UNKNOWN;
return;
}
}
void
306 WaypointFollower::goalResponseCallback(
307 const rclcpp_action::ClientGoalHandle<ClientT>::SharedPtr & goal )
{
if ( !goal ) {
RCLCPP_ERROR(
get_logger( ),
"navigate_to_pose action client failed to send goal to server." );
current_goal_status_ = ActionStatus::FAILED;
}
}
rcl_interfaces::msg::SetParametersResult
318 WaypointFollower::dynamicParametersCallback( std::vector<rclcpp::Parameter> parameters )
{
// No locking required as action server is running on same single threaded executor
rcl_interfaces::msg::SetParametersResult result;
for ( auto parameter : parameters ) {
const auto & type = parameter.get_type( );
const auto & name = parameter.get_name( );
if ( type == ParameterType::PARAMETER_INTEGER ) {
if ( name == "loop_rate" ) {
loop_rate_ = parameter.as_int( );
}
} else if ( type == ParameterType::PARAMETER_BOOL ) {
if ( name == "stop_on_failure" ) {
stop_on_failure_ = parameter.as_bool( );
}
}
}
result.successful = true;
return result;
}
} // namespace nav2_waypoint_follower
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
349 RCLCPP_COMPONENTS_REGISTER_NODE( nav2_waypoint_follower::WaypointFollower )
1 // Copyright ( c ) 2021, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <math.h>
#include <memory>
#include <string>
#include <vector>
#include "gtest/gtest.h"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_waypoint_follower/waypoint_follower.hpp"
#include "rclcpp/rclcpp.hpp"
26 class WPShim : public nav2_waypoint_follower::WaypointFollower
{
public:
29 WPShim( )
: nav2_waypoint_follower::WaypointFollower( rclcpp::NodeOptions( ) )
{
}
34 void configure( )
{
rclcpp_lifecycle::State state;
this->on_configure( state );
}
40 void activate( )
{
rclcpp_lifecycle::State state;
this->on_activate( state );
}
};
47 class RclCppFixture
{
public:
50 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
51 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
55 TEST( WPTest, test_dynamic_parameters )
{
auto follower = std::make_shared<WPShim>( );
follower->configure( );
follower->activate( );
auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
follower->get_node_base_interface( ), follower->get_node_topics_interface( ),
follower->get_node_graph_interface( ),
follower->get_node_services_interface( ) );
auto results = rec_param->set_parameters_atomically(
{rclcpp::Parameter( "loop_rate", 100 ),
rclcpp::Parameter( "stop_on_failure", false )} );
rclcpp::spin_until_future_complete(
follower->get_node_base_interface( ),
results );
EXPECT_EQ( follower->get_parameter( "loop_rate" ).as_int( ), 100 );
EXPECT_EQ( follower->get_parameter( "stop_on_failure" ).as_bool( ), false );
}
1 // Copyright ( c ) 2021, Samsung Research America
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License. Reserved.
#include <math.h>
#include <condition_variable>
#include <memory>
#include <string>
#include <vector>
#include <utility>
#include <thread>
#include "gtest/gtest.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_waypoint_follower/plugins/photo_at_waypoint.hpp"
#include "nav2_waypoint_follower/plugins/wait_at_waypoint.hpp"
#include "nav2_waypoint_follower/plugins/input_at_waypoint.hpp"
31 class RclCppFixture
{
public:
34 RclCppFixture( ) {rclcpp::init( 0, nullptr );}
35 ~RclCppFixture( ) {rclcpp::shutdown( );}
};
RclCppFixture g_rclcppfixture;
39 TEST( WaypointFollowerTest, WaitAtWaypoint )
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>( "testWaypointNode" );
node->declare_parameter( "WAW.waypoint_pause_duration", 50 );
std::unique_ptr<nav2_waypoint_follower::WaitAtWaypoint> waw(
new nav2_waypoint_follower::WaitAtWaypoint
);
waw->initialize( node, std::string( "WAW" ) );
auto start_time = node->now( );
// should wait 50ms
geometry_msgs::msg::PoseStamped pose;
waw->processAtWaypoint( pose, 0 );
auto end_time = node->now( );
EXPECT_NEAR( ( end_time - start_time ).seconds( ), 0.05, 0.01 );
waw.reset( new nav2_waypoint_follower::WaitAtWaypoint );
node->set_parameter( rclcpp::Parameter( "WAW.enabled", false ) );
waw->initialize( node, std::string( "WAW" ) );
// plugin is not enabled, should exit
EXPECT_TRUE( waw->processAtWaypoint( pose, 0 ) );
}
68 TEST( WaypointFollowerTest, InputAtWaypoint )
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>( "testWaypointNode" );
auto pub = node->create_publisher<std_msgs::msg::Empty>( "input_at_waypoint/input", 1 );
pub->on_activate( );
auto publish_message =
[&, this]( ) -> void
{
rclcpp::Rate( 5 ).sleep( );
auto msg = std::make_unique<std_msgs::msg::Empty>( );
pub->publish( std::move( msg ) );
rclcpp::spin_some( node->shared_from_this( )->get_node_base_interface( ) );
};
std::unique_ptr<nav2_waypoint_follower::InputAtWaypoint> iaw(
new nav2_waypoint_follower::InputAtWaypoint
);
iaw->initialize( node, std::string( "IAW" ) );
auto start_time = node->now( );
// no input, should timeout
geometry_msgs::msg::PoseStamped pose;
EXPECT_FALSE( iaw->processAtWaypoint( pose, 0 ) );
auto end_time = node->now( );
EXPECT_NEAR( ( end_time - start_time ).seconds( ), 10.0, 0.1 );
// has input now, should work
std::thread t1( publish_message );
EXPECT_TRUE( iaw->processAtWaypoint( pose, 0 ) );
t1.join( );
iaw.reset( new nav2_waypoint_follower::InputAtWaypoint );
node->set_parameter( rclcpp::Parameter( "IAW.enabled", false ) );
iaw->initialize( node, std::string( "IAW" ) );
// plugin is not enabled, should exit
EXPECT_TRUE( iaw->processAtWaypoint( pose, 0 ) );
}
110 TEST( WaypointFollowerTest, PhotoAtWaypoint )
{
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>( "testWaypointNode" );
auto pub = node->create_publisher<sensor_msgs::msg::Image>( "/camera/color/image_raw", 1 );
pub->on_activate( );
std::condition_variable cv;
std::mutex mtx;
std::unique_lock<std::mutex> lck( mtx, std::defer_lock );
bool data_published = false;
auto publish_message =
[&, this]( ) -> void
{
rclcpp::Rate( 5 ).sleep( );
auto msg = std::make_unique<sensor_msgs::msg::Image>( );
// fill image msg data.
msg->encoding = "rgb8";
msg->height = 240;
msg->width = 320;
msg->step = 960;
auto size = msg->height * msg->width * 3;
msg->data.reserve( size );
int fake_data = 0;
for ( size_t i = 0; i < size; i++ ) {
msg->data.push_back( fake_data++ );
}
pub->publish( std::move( msg ) );
rclcpp::spin_some( node->shared_from_this( )->get_node_base_interface( ) );
lck.lock( );
data_published = true;
cv.notify_one( );
lck.unlock( );
};
std::unique_ptr<nav2_waypoint_follower::PhotoAtWaypoint> paw(
new nav2_waypoint_follower::PhotoAtWaypoint
);
paw->initialize( node, std::string( "PAW" ) );
// no images, throws because can't write
geometry_msgs::msg::PoseStamped pose;
EXPECT_FALSE( paw->processAtWaypoint( pose, 0 ) );
std::thread t1( publish_message );
cv.wait( lck );
// has image now, since we force waiting until image is published
EXPECT_TRUE( paw->processAtWaypoint( pose, 0 ) );
t1.join( );
paw.reset( new nav2_waypoint_follower::PhotoAtWaypoint );
node->set_parameter( rclcpp::Parameter( "PAW.enabled", false ) );
paw->initialize( node, std::string( "PAW" ) );
// plugin is not enabled, should exit
EXPECT_TRUE( paw->processAtWaypoint( pose, 0 ) );
}
1 // Copyright ( c ) 2022, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_
#define CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_
#include <vector>
#include "controller_interface/controller_interface_base.hpp"
#include "controller_interface/visibility_control.h"
#include "hardware_interface/handle.hpp"
namespace controller_interface
{
/// Virtual class to implement when integrating a controller that can be preceded by other
/// controllers.
/**
* Specialization of ControllerInterface class to force implementation of methods specific for
* "chainable" controller, i.e., controller that can be preceded by an another controller, for
* example inner controller of an control cascade.
*
*/
34 class ChainableControllerInterface : public ControllerInterfaceBase
{
public:
CONTROLLER_INTERFACE_PUBLIC
38 ChainableControllerInterface( );
CONTROLLER_INTERFACE_PUBLIC
virtual ~ChainableControllerInterface( ) = default;
CONTROLLER_INTERFACE_PUBLIC
return_type update( const rclcpp::Time & time, const rclcpp::Duration & period ) final;
CONTROLLER_INTERFACE_PUBLIC
bool is_chainable( ) const final;
CONTROLLER_INTERFACE_PUBLIC
std::vector<hardware_interface::CommandInterface> export_reference_interfaces( ) final;
CONTROLLER_INTERFACE_PUBLIC
bool set_chained_mode( bool chained_mode ) final;
CONTROLLER_INTERFACE_PUBLIC
bool is_in_chained_mode( ) const final;
protected:
/// Virtual method that each chainable controller should implement to export its chainable
/// interfaces.
/**
* Each chainable controller implements this methods where all input ( command ) interfaces are
* exported. The method has the same meaning as `export_command_interface` method from
* hardware_interface::SystemInterface or hardware_interface::ActuatorInterface.
*
* \returns list of CommandInterfaces that other controller can use as their outputs.
*/
virtual std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces( ) = 0;
/// Virtual method that each chainable controller should implement to switch chained mode.
/**
* Each chainable controller implements this methods to switch between "chained" and "external"
* mode. In "chained" mode all external interfaces like subscriber and service servers are
* disabled to avoid potential concurrency in input commands.
*
* \param[in] flag marking a switch to or from chained mode.
*
* \returns true if controller successfully switched between "chained" and "external" mode. \default returns true so the method don't have to be overridden if controller can always switch chained mode.
*/
virtual bool on_set_chained_mode( bool chained_mode );
/// Update reference from input topics when not in chained mode.
/**
* Each chainable controller implements this method to update reference from subscribers when not
* in chained mode.
*
* \returns return_type::OK if update is successfully, otherwise return_type::ERROR.
*/
virtual return_type update_reference_from_subscribers( ) = 0;
/// Execute calculations of the controller and update command interfaces.
/**
* Update method for chainable controllers.
* In this method is valid to assume that \reference_interfaces_ hold the values for calculation
* of the commands in the current control step.
* This means that this method is called after \update_reference_from_subscribers if controller is
* not in chained mode.
*
* \returns return_type::OK if calculation and writing of interface is successfully, otherwise
* return_type::ERROR.
*/
virtual return_type update_and_write_commands(
const rclcpp::Time & time, const rclcpp::Duration & period ) = 0;
/// Storage of values for reference interfaces
std::vector<double> reference_interfaces_;
private:
/// A flag marking if a chainable controller is currently preceded by another controller.
bool in_chained_mode_ = false;
};
} // namespace controller_interface
#endif // CONTROLLER_INTERFACE__CHAINABLE_CONTROLLER_INTERFACE_HPP_
1 // Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_HPP_
#define CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_HPP_
#include <memory>
#include <string>
#include <vector>
#include "controller_interface/controller_interface_base.hpp"
#include "controller_interface/visibility_control.h"
#include "hardware_interface/handle.hpp"
namespace controller_interface
{
28 class ControllerInterface : public controller_interface::ControllerInterfaceBase
{
public:
CONTROLLER_INTERFACE_PUBLIC
32 ControllerInterface( );
CONTROLLER_INTERFACE_PUBLIC
virtual ~ControllerInterface( ) = default;
/**
* Controller is not chainable.
*
* \returns false.
*/
CONTROLLER_INTERFACE_PUBLIC
bool is_chainable( ) const final;
/**
* Controller has no reference interfaces.
*
* \returns empty list.
*/
CONTROLLER_INTERFACE_PUBLIC
std::vector<hardware_interface::CommandInterface> export_reference_interfaces( ) final;
/**
* Controller is not chainable, therefore no chained mode can be set.
*
* \returns false.
*/
CONTROLLER_INTERFACE_PUBLIC
bool set_chained_mode( bool chained_mode ) final;
/**
* Controller can not be in chained mode.
*
* \returns false.
*/
CONTROLLER_INTERFACE_PUBLIC
bool is_in_chained_mode( ) const final;
};
using ControllerInterfaceSharedPtr = std::shared_ptr<ControllerInterface>;
} // namespace controller_interface
#endif // CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_HPP_
// Copyright ( c ) 2022, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_BASE_HPP_
#define CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_BASE_HPP_
#include <memory>
#include <string>
#include <vector>
#include "controller_interface/visibility_control.h"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/loaned_command_interface.hpp"
#include "hardware_interface/loaned_state_interface.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
namespace controller_interface
{
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
35 enum class return_type : std::uint8_t
{
OK = 0,
ERROR = 1,
};
/// Indicating which interfaces are to be claimed.
/**
* One might either claim all available command/state interfaces,
* specifying a set of individual interfaces,
* or none at all.
*/
47 enum class interface_configuration_type : std::uint8_t
{
ALL = 0,
INDIVIDUAL = 1,
NONE = 2,
};
/// Configuring what command/state interfaces to claim.
struct InterfaceConfiguration
{
interface_configuration_type type;
std::vector<std::string> names = {};
};
/**
* Base interface class for an controller. The interface may not be used to implement a controller.
* The class provides definitions for `ControllerInterface` and `ChainableControllerInterface`
* that should be implemented and extended for a specific controller.
*/
66 class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
{
public:
CONTROLLER_INTERFACE_PUBLIC
ControllerInterfaceBase( ) = default;
CONTROLLER_INTERFACE_PUBLIC
virtual ~ControllerInterfaceBase( ) = default;
CONTROLLER_INTERFACE_PUBLIC
virtual InterfaceConfiguration command_interface_configuration( ) const = 0;
CONTROLLER_INTERFACE_PUBLIC
virtual InterfaceConfiguration state_interface_configuration( ) const = 0;
CONTROLLER_INTERFACE_PUBLIC
void assign_interfaces(
std::vector<hardware_interface::LoanedCommandInterface> && command_interfaces,
std::vector<hardware_interface::LoanedStateInterface> && state_interfaces );
CONTROLLER_INTERFACE_PUBLIC
void release_interfaces( );
CONTROLLER_INTERFACE_PUBLIC
virtual return_type init(
const std::string & controller_name, const std::string & namespace_ = "",
const rclcpp::NodeOptions & node_options =
rclcpp::NodeOptions( )
.allow_undeclared_parameters( true )
.automatically_declare_parameters_from_overrides( true ) );
/// Custom configure method to read additional parameters for controller-nodes
/*
* Override default implementation for configure of LifecycleNode to get parameters.
*/
CONTROLLER_INTERFACE_PUBLIC
const rclcpp_lifecycle::State & configure( );
/// Extending interface with initialization method which is individual for each controller
CONTROLLER_INTERFACE_PUBLIC
virtual CallbackReturn on_init( ) = 0;
CONTROLLER_INTERFACE_PUBLIC
virtual return_type update( const rclcpp::Time & time, const rclcpp::Duration & period ) = 0;
CONTROLLER_INTERFACE_PUBLIC
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> get_node( );
CONTROLLER_INTERFACE_PUBLIC
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> get_node( ) const;
CONTROLLER_INTERFACE_PUBLIC
const rclcpp_lifecycle::State & get_state( ) const;
CONTROLLER_INTERFACE_PUBLIC
unsigned int get_update_rate( ) const;
/// Declare and initialize a parameter with a type.
/**
*
* Wrapper function for templated node's declare_parameter( ) which checks if
* parameter is already declared.
* For use in all components that inherit from ControllerInterfaceBase
*/
template <typename ParameterT>
auto auto_declare( const std::string & name, const ParameterT & default_value )
{
if ( !node_->has_parameter( name ) )
{
return node_->declare_parameter<ParameterT>( name, default_value );
}
else
{
return node_->get_parameter( name ).get_value<ParameterT>( );
}
}
// Methods for chainable controller types with default values so we can put all controllers into
// one list in Controller Manager
/// Get information if a controller is chainable.
/**
* Get information if a controller is chainable.
*
* \returns true is controller is chainable and false if it is not.
*/
CONTROLLER_INTERFACE_PUBLIC
virtual bool is_chainable( ) const = 0;
/**
* Export interfaces for a chainable controller that can be used as command interface of other
* controllers.
*
* \returns list of command interfaces for preceding controllers.
*/
CONTROLLER_INTERFACE_PUBLIC
virtual std::vector<hardware_interface::CommandInterface> export_reference_interfaces( ) = 0;
/**
* Set chained mode of a chainable controller. This method triggers internal processes to switch
* a chainable controller to "chained" mode and vice-versa. Setting controller to "chained" mode
* usually involves disabling of subscribers and other external interfaces to avoid potential
* concurrency in input commands.
*
* \returns true if mode is switched successfully and false if not.
*/
CONTROLLER_INTERFACE_PUBLIC
virtual bool set_chained_mode( bool chained_mode ) = 0;
/// Get information if a controller is currently in chained mode.
/**
* Get information about controller if it is currently used in chained mode. In chained mode only
* internal interfaces are available and all subscribers are expected to be disabled. This
* prevents concurrent writing to controller's inputs from multiple sources.
*
* \returns true is controller is in chained mode and false if it is not.
*/
CONTROLLER_INTERFACE_PUBLIC
virtual bool is_in_chained_mode( ) const = 0;
protected:
std::vector<hardware_interface::LoanedCommandInterface> command_interfaces_;
std::vector<hardware_interface::LoanedStateInterface> state_interfaces_;
unsigned int update_rate_ = 0;
private:
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
};
using ControllerInterfaceBaseSharedPtr = std::shared_ptr<ControllerInterfaceBase>;
} // namespace controller_interface
#endif // CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_BASE_HPP_
1 // Copyright ( c ) 2021, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef CONTROLLER_INTERFACE__HELPERS_HPP_
#define CONTROLLER_INTERFACE__HELPERS_HPP_
#include <functional>
#include <string>
#include <vector>
namespace controller_interface
{
/// Reorder interfaces with references according to joint names or full interface names.
/**
* Method to reorder and check if all expected interfaces are provided for the joint.
* Fill `ordered_interfaces` with references from `unordered_interfaces` in the same order as in
* `ordered_names`.
*
* \param[in] unordered_interfaces vector with loaned unordered state or command interfaces.
* \param[in] ordered_names vector with ordered names to order \p unordered_interfaces.
* The valued inputs are list of joint names or interface full names.
* If joint names are used for ordering, \p interface_type specifies valid interface.
* If full interface names are used for ordering, \p interface_type should be empty string ( "" ).
* \param[in] interface_type used for ordering interfaces with respect to joint names.
* \param[out] ordered_interfaces vector with ordered interfaces.
* \return true if all interfaces or joints in \p ordered_names are found, otherwise false.
*/
template <typename T>
40 bool get_ordered_interfaces(
std::vector<T> & unordered_interfaces, const std::vector<std::string> & ordered_names,
const std::string & interface_type, std::vector<std::reference_wrapper<T>> & ordered_interfaces )
{
ordered_interfaces.reserve( ordered_names.size( ) );
for ( const auto & name : ordered_names )
{
for ( auto & interface : unordered_interfaces )
{
if ( !interface_type.empty( ) )
{
// check case where:
// ( <joint> == <joint> AND <interface> == <interface> ) OR <joint>/<interface> == 'full name'
if (
( ( name == interface.get_prefix_name( ) ) &&
( interface_type == interface.get_interface_name( ) ) ) ||
( ( name + "/" + interface_type ) == interface.get_name( ) ) )
{
ordered_interfaces.push_back( std::ref( interface ) );
}
}
else
{
if ( name == interface.get_name( ) )
{
ordered_interfaces.push_back( std::ref( interface ) );
}
}
}
}
return ordered_names.size( ) == ordered_interfaces.size( );
}
74 inline bool interface_list_contains_interface_type(
const std::vector<std::string> & interface_type_list, const std::string & interface_type )
{
return std::find( interface_type_list.begin( ), interface_type_list.end( ), interface_type ) !=
interface_type_list.end( );
}
} // namespace controller_interface
#endif // CONTROLLER_INTERFACE__HELPERS_HPP_
1 // Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef CONTROLLER_INTERFACE__VISIBILITY_CONTROL_H_
#define CONTROLLER_INTERFACE__VISIBILITY_CONTROL_H_
// This logic was borrowed ( then namespaced ) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define CONTROLLER_INTERFACE_EXPORT __attribute__( ( dllexport ) )
#define CONTROLLER_INTERFACE_IMPORT __attribute__( ( dllimport ) )
#else
#define CONTROLLER_INTERFACE_EXPORT __declspec( dllexport )
#define CONTROLLER_INTERFACE_IMPORT __declspec( dllimport )
#endif
#ifdef CONTROLLER_INTERFACE_BUILDING_DLL
#define CONTROLLER_INTERFACE_PUBLIC CONTROLLER_INTERFACE_EXPORT
#else
#define CONTROLLER_INTERFACE_PUBLIC CONTROLLER_INTERFACE_IMPORT
#endif
#define CONTROLLER_INTERFACE_PUBLIC_TYPE CONTROLLER_INTERFACE_PUBLIC
#define CONTROLLER_INTERFACE_LOCAL
#else
#define CONTROLLER_INTERFACE_EXPORT __attribute__( ( visibility( "default" ) ) )
#define CONTROLLER_INTERFACE_IMPORT
#if __GNUC__ >= 4
#define CONTROLLER_INTERFACE_PUBLIC __attribute__( ( visibility( "default" ) ) )
#define CONTROLLER_INTERFACE_LOCAL __attribute__( ( visibility( "hidden" ) ) )
#else
#define CONTROLLER_INTERFACE_PUBLIC
#define CONTROLLER_INTERFACE_LOCAL
#endif
#define CONTROLLER_INTERFACE_PUBLIC_TYPE
#endif
#endif // CONTROLLER_INTERFACE__VISIBILITY_CONTROL_H_
// Copyright ( c ) 2021, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef SEMANTIC_COMPONENTS__FORCE_TORQUE_SENSOR_HPP_
#define SEMANTIC_COMPONENTS__FORCE_TORQUE_SENSOR_HPP_
#include <limits>
#include <string>
#include <vector>
#include "geometry_msgs/msg/wrench.hpp"
#include "hardware_interface/loaned_state_interface.hpp"
#include "semantic_components/semantic_component_interface.hpp"
namespace semantic_components
{
28 class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::Wrench>
{
public:
/// Constructor for "standard" 6D FTS
32 explicit ForceTorqueSensor( const std::string & name ) : SemanticComponentInterface( name, 6 )
{
// If 6D FTS use standard names
interface_names_.emplace_back( name_ + "/" + "force.x" );
interface_names_.emplace_back( name_ + "/" + "force.y" );
interface_names_.emplace_back( name_ + "/" + "force.z" );
interface_names_.emplace_back( name_ + "/" + "torque.x" );
interface_names_.emplace_back( name_ + "/" + "torque.y" );
interface_names_.emplace_back( name_ + "/" + "torque.z" );
// Set all interfaces existing
std::fill( existing_axes_.begin( ), existing_axes_.end( ), true );
// Set default force and torque values to NaN
std::fill( forces_.begin( ), forces_.end( ), std::numeric_limits<double>::quiet_NaN( ) );
std::fill( torques_.begin( ), torques_.end( ), std::numeric_limits<double>::quiet_NaN( ) );
}
/// Constructor for 6D FTS with custom interface names.
/**
* Constructor for 6D FTS with custom interface names or FTS with less then six measurement axes,
* e.g., 1D and 2D force load cells.
* For non existing axes interface is empty string, i.e., ( "" );
*
* The name should be in the following order:
* force X, force Y, force Z, torque X, torque Y, torque Z.
*/
59 ForceTorqueSensor(
60 const std::string & interface_force_x, const std::string & interface_force_y,
61 const std::string & interface_force_z, const std::string & interface_torque_x,
62 const std::string & interface_torque_y, const std::string & interface_torque_z )
: SemanticComponentInterface( "", 6 )
{
auto check_and_add_interface = [this]( const std::string & interface_name, const int index )
{
if ( !interface_name.empty( ) )
{
interface_names_.emplace_back( interface_name );
existing_axes_[index] = true;
}
else
{
existing_axes_[index] = false;
}
};
check_and_add_interface( interface_force_x, 0 );
check_and_add_interface( interface_force_y, 1 );
check_and_add_interface( interface_force_z, 2 );
check_and_add_interface( interface_torque_x, 3 );
check_and_add_interface( interface_torque_y, 4 );
check_and_add_interface( interface_torque_z, 5 );
// Set default force and torque values to NaN
std::fill( forces_.begin( ), forces_.end( ), std::numeric_limits<double>::quiet_NaN( ) );
std::fill( torques_.begin( ), torques_.end( ), std::numeric_limits<double>::quiet_NaN( ) );
}
virtual ~ForceTorqueSensor( ) = default;
/// Return forces.
/**
* Return forces of a FTS.
*
* \return array of size 3 with force values.
*/
std::array<double, 3> get_forces( )
{
size_t interface_counter = 0;
for ( size_t i = 0; i < 3; ++i )
{
if ( existing_axes_[i] )
{
forces_[i] = state_interfaces_[interface_counter].get( ).get_value( );
++interface_counter;
}
}
return forces_;
}
/// Return torque.
/**
* Return torques of a FTS.
*
* \return array of size 3 with torque values.
*/
118 std::array<double, 3> get_torques( )
{
// find out how many force interfaces are being used
// torque interfaces will be found from the next index onward
auto torque_interface_counter =
std::count( existing_axes_.begin( ), existing_axes_.begin( ) + 3, true );
for ( size_t i = 3; i < 6; ++i )
{
if ( existing_axes_[i] )
{
torques_[i - 3] = state_interfaces_[torque_interface_counter].get( ).get_value( );
++torque_interface_counter;
}
}
return torques_;
}
/// Return Wrench message with forces and torques.
/**
* Constructs and return a wrench message from the current values.
* The method assumes that the interface names on the construction are in the following order:
* force X, force Y, force Z, torque X, torque Y, torque Z.
*
* \return wrench message from values;
*/
144 bool get_values_as_message( geometry_msgs::msg::Wrench & message )
{
// call get_forces( ) and get_troque( ) to update with the latest values
get_forces( );
get_torques( );
// update the message values
message.force.x = forces_[0];
message.force.y = forces_[1];
message.force.z = forces_[2];
message.torque.x = torques_[0];
message.torque.y = torques_[1];
message.torque.z = torques_[2];
return true;
}
protected:
/// Vector with existing axes for sensors with less then 6D axes.
// Order is: force X, force Y, force Z, torque X, torque Y, torque Z.
std::array<bool, 6> existing_axes_;
std::array<double, 3> forces_;
std::array<double, 3> torques_;
};
} // namespace semantic_components
#endif // SEMANTIC_COMPONENTS__FORCE_TORQUE_SENSOR_HPP_
// Copyright 2021 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef SEMANTIC_COMPONENTS__IMU_SENSOR_HPP_
#define SEMANTIC_COMPONENTS__IMU_SENSOR_HPP_
#include <limits>
#include <string>
#include <vector>
#include "semantic_components/semantic_component_interface.hpp"
#include "sensor_msgs/msg/imu.hpp"
namespace semantic_components
{
27 class IMUSensor : public SemanticComponentInterface<sensor_msgs::msg::Imu>
{
public:
30 explicit IMUSensor( const std::string & name ) : SemanticComponentInterface( name, 10 )
{
interface_names_.emplace_back( name_ + "/" + "orientation.x" );
interface_names_.emplace_back( name_ + "/" + "orientation.y" );
interface_names_.emplace_back( name_ + "/" + "orientation.z" );
interface_names_.emplace_back( name_ + "/" + "orientation.w" );
interface_names_.emplace_back( name_ + "/" + "angular_velocity.x" );
interface_names_.emplace_back( name_ + "/" + "angular_velocity.y" );
interface_names_.emplace_back( name_ + "/" + "angular_velocity.z" );
interface_names_.emplace_back( name_ + "/" + "linear_acceleration.x" );
interface_names_.emplace_back( name_ + "/" + "linear_acceleration.y" );
interface_names_.emplace_back( name_ + "/" + "linear_acceleration.z" );
// Set default values to NaN
orientation_.fill( std::numeric_limits<double>::quiet_NaN( ) );
angular_velocity_.fill( std::numeric_limits<double>::quiet_NaN( ) );
linear_acceleration_.fill( std::numeric_limits<double>::quiet_NaN( ) );
}
virtual ~IMUSensor( ) = default;
/// Return orientation.
/**
* Return orientation reported by an IMU
*
* \return array of size 4 with orientation quaternion ( x, y, z, w )
*/
std::array<double, 4> get_orientation( )
{
size_t interface_offset = 0;
for ( size_t i = 0; i < orientation_.size( ); ++i )
{
orientation_[i] = state_interfaces_[interface_offset + i].get( ).get_value( );
}
return orientation_;
}
/// Return angular velocity.
/**
* Return angular velocity reported by an IMU
*
* \return array of size 3 with angular velocity values.
*/
73 std::array<double, 3> get_angular_velocity( )
{
size_t interface_offset = orientation_.size( );
for ( size_t i = 0; i < angular_velocity_.size( ); ++i )
{
angular_velocity_[i] = state_interfaces_[interface_offset + i].get( ).get_value( );
}
return angular_velocity_;
}
/// Return linear acceleration.
/**
* Return linear acceleration reported by an IMU
*
* \return array of size 3 with linear acceleration values.
*/
89 std::array<double, 3> get_linear_acceleration( )
{
size_t interface_offset = orientation_.size( ) + angular_velocity_.size( );
for ( size_t i = 0; i < linear_acceleration_.size( ); ++i )
{
linear_acceleration_[i] = state_interfaces_[interface_offset + i].get( ).get_value( );
}
return linear_acceleration_;
}
/// Return Imu message with orientation, angular velocity and linear acceleration
/**
* Constructs and return a IMU message from the current values.
* \return imu message from values;
*/
104 bool get_values_as_message( sensor_msgs::msg::Imu & message )
{
// call get_orientation( ) and get_angular_velocity( ) get_linear_acceleration( ) to
// update with the latest values
get_orientation( );
get_angular_velocity( );
get_linear_acceleration( );
// update the message values, covariances unknown
message.orientation.x = orientation_[0];
message.orientation.y = orientation_[1];
message.orientation.z = orientation_[2];
message.orientation.w = orientation_[3];
message.orientation_covariance.fill( .0 );
message.angular_velocity.x = angular_velocity_[0];
message.angular_velocity.y = angular_velocity_[1];
message.angular_velocity.z = angular_velocity_[2];
message.angular_velocity_covariance.fill( .0 );
message.linear_acceleration.x = linear_acceleration_[0];
message.linear_acceleration.y = linear_acceleration_[1];
message.linear_acceleration.z = linear_acceleration_[2];
message.linear_acceleration_covariance.fill( .0 );
return true;
}
protected:
// Order is: orientation X, Y, Z, W angular velocity X, Y, Z and linear acceleration X, Y, Z
std::array<double, 4> orientation_;
std::array<double, 3> angular_velocity_;
std::array<double, 3> linear_acceleration_;
};
} // namespace semantic_components
#endif // SEMANTIC_COMPONENTS__IMU_SENSOR_HPP_
// Copyright ( c ) 2021, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef SEMANTIC_COMPONENTS__SEMANTIC_COMPONENT_INTERFACE_HPP_
#define SEMANTIC_COMPONENTS__SEMANTIC_COMPONENT_INTERFACE_HPP_
#include <string>
#include <vector>
#include "controller_interface/helpers.hpp"
#include "hardware_interface/loaned_state_interface.hpp"
namespace semantic_components
{
template <typename MessageReturnType>
27 class SemanticComponentInterface
{
public:
30 explicit SemanticComponentInterface( const std::string & name, size_t size = 0 )
{
name_ = name;
interface_names_.reserve( size );
state_interfaces_.reserve( size );
}
~SemanticComponentInterface( ) = default;
/// Assign loaned state interfaces from the hardware.
/**
* Assign loaned state interfaces on the controller start.
*
* \param[in] state_interfaces vector of interfaces provided by the controller.
*/
bool assign_loaned_state_interfaces(
std::vector<hardware_interface::LoanedStateInterface> & state_interfaces )
{
return controller_interface::get_ordered_interfaces(
state_interfaces, interface_names_, "", state_interfaces_ );
}
/// Release loaned interfaces from the hardware.
void release_interfaces( ) { state_interfaces_.clear( ); }
/// Definition of state interface names for the component.
/**
* The function should be used in "state_interface_configuration( )" of a controller to provide
* standardized interface names semantic component.
*
* \default Default implementation defined state interfaces as "name/NR" where NR is number
* from 0 to size of values;
* \return list of strings with state interface names for the semantic component.
*/
64 virtual std::vector<std::string> get_state_interface_names( )
{
if ( interface_names_.empty( ) )
{
for ( auto i = 0u; i < interface_names_.capacity( ); ++i )
{
interface_names_.emplace_back( name_ + "/" + std::to_string( i + 1 ) );
}
}
return interface_names_;
}
/// Return all values.
/**
* \return true if it gets all the values, else false
*/
80 bool get_values( std::vector<double> & values ) const
{
// check we have sufficient memory
if ( values.capacity( ) != state_interfaces_.size( ) )
{
return false;
}
// insert all the values
for ( size_t i = 0; i < state_interfaces_.size( ); ++i )
{
values.emplace_back( state_interfaces_[i].get( ).get_value( ) );
}
return true;
}
/// Return values as MessageReturnType
/**
* \return false by default
*/
99 bool get_values_as_message( MessageReturnType & /* message */ ) { return false; }
protected:
std::string name_;
std::vector<std::string> interface_names_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>> state_interfaces_;
};
} // namespace semantic_components
#endif // SEMANTIC_COMPONENTS__SEMANTIC_COMPONENT_INTERFACE_HPP_
1 // Copyright ( c ) 2022, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "controller_interface/chainable_controller_interface.hpp"
#include <vector>
#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "lifecycle_msgs/msg/state.hpp"
namespace controller_interface
{
24 ChainableControllerInterface::ChainableControllerInterface( ) : ControllerInterfaceBase( ) {}
26 bool ChainableControllerInterface::is_chainable( ) const { return true; }
28 return_type ChainableControllerInterface::update(
29 const rclcpp::Time & time, const rclcpp::Duration & period )
{
return_type ret = return_type::ERROR;
if ( !is_in_chained_mode( ) )
{
ret = update_reference_from_subscribers( );
if ( ret != return_type::OK )
{
return ret;
}
}
ret = update_and_write_commands( time, period );
return ret;
}
std::vector<hardware_interface::CommandInterface>
48 ChainableControllerInterface::export_reference_interfaces( )
{
auto reference_interfaces = on_export_reference_interfaces( );
// check if the "reference_interfaces_" variable is resized to number of interfaces
if ( reference_interfaces_.size( ) != reference_interfaces.size( ) )
{
// TODO( destogl ): Should here be "FATAL"? It is fatal in terms of controller but not for the
// framework
RCLCPP_FATAL(
get_node( )->get_logger( ),
"The internal storage for reference values 'reference_interfaces_' variable has size '%zu', "
"but it is expected to have the size '%zu' equal to the number of exported reference "
"interfaces. No reference interface will be exported. Please correct and recompile "
"the controller with name '%s' and try again.",
reference_interfaces_.size( ), reference_interfaces.size( ), get_node( )->get_name( ) );
reference_interfaces.clear( );
}
// check if the names of the reference interfaces begin with the controller's name
for ( const auto & interface : reference_interfaces )
{
if ( interface.get_prefix_name( ) != get_node( )->get_name( ) )
{
RCLCPP_FATAL(
get_node( )->get_logger( ),
"The name of the interface '%s' does not begin with the controller's name. This is "
"mandatory "
" for reference interfaces. No reference interface will be exported. Please correct and "
"recompile the controller with name '%s' and try again.",
interface.get_name( ).c_str( ), get_node( )->get_name( ) );
reference_interfaces.clear( );
break;
}
}
return reference_interfaces;
}
87 bool ChainableControllerInterface::set_chained_mode( bool chained_mode )
{
bool result = false;
if ( get_state( ).id( ) != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
{
result = on_set_chained_mode( chained_mode );
if ( result )
{
in_chained_mode_ = chained_mode;
}
}
else
{
RCLCPP_ERROR(
get_node( )->get_logger( ),
"Can not change controller's chained mode because it is no in '%s' state. "
"Current state is '%s'.",
hardware_interface::lifecycle_state_names::UNCONFIGURED, get_state( ).label( ).c_str( ) );
}
return result;
}
112 bool ChainableControllerInterface::is_in_chained_mode( ) const { return in_chained_mode_; }
114 bool ChainableControllerInterface::on_set_chained_mode( bool /*chained_mode*/ ) { return true; }
} // namespace controller_interface
1 // Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "controller_interface/controller_interface.hpp"
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "lifecycle_msgs/msg/state.hpp"
namespace controller_interface
{
27 ControllerInterface::ControllerInterface( ) : ControllerInterfaceBase( ) {}
29 bool ControllerInterface::is_chainable( ) const { return false; }
31 std::vector<hardware_interface::CommandInterface> ControllerInterface::export_reference_interfaces( )
{
return {};
}
36 bool ControllerInterface::set_chained_mode( bool /*chained_mode*/ ) { return false; }
38 bool ControllerInterface::is_in_chained_mode( ) const { return false; }
} // namespace controller_interface
1 // Copyright ( c ) 2022, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "controller_interface/controller_interface_base.hpp"
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "lifecycle_msgs/msg/state.hpp"
namespace controller_interface
{
27 return_type ControllerInterfaceBase::init(
28 const std::string & controller_name, const std::string & namespace_,
29 const rclcpp::NodeOptions & node_options )
{
node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
controller_name, namespace_, node_options, false ); // disable LifecycleNode service interfaces
try
{
auto_declare<int>( "update_rate", 0 );
}
catch ( const std::exception & e )
{
fprintf( stderr, "Exception thrown during init stage with message: %s \n", e.what( ) );
return return_type::ERROR;
}
switch ( on_init( ) )
{
case LifecycleNodeInterface::CallbackReturn::SUCCESS:
break;
case LifecycleNodeInterface::CallbackReturn::ERROR:
case LifecycleNodeInterface::CallbackReturn::FAILURE:
return return_type::ERROR;
}
node_->register_on_configure(
std::bind( &ControllerInterfaceBase::on_configure, this, std::placeholders::_1 ) );
node_->register_on_cleanup(
std::bind( &ControllerInterfaceBase::on_cleanup, this, std::placeholders::_1 ) );
node_->register_on_activate(
std::bind( &ControllerInterfaceBase::on_activate, this, std::placeholders::_1 ) );
node_->register_on_deactivate(
std::bind( &ControllerInterfaceBase::on_deactivate, this, std::placeholders::_1 ) );
node_->register_on_shutdown(
std::bind( &ControllerInterfaceBase::on_shutdown, this, std::placeholders::_1 ) );
node_->register_on_error(
std::bind( &ControllerInterfaceBase::on_error, this, std::placeholders::_1 ) );
return return_type::OK;
}
74 const rclcpp_lifecycle::State & ControllerInterfaceBase::configure( )
{
// TODO( destogl ): this should actually happen in "on_configure" but I am not sure how to get
// overrides correctly in combination with std::bind. The goal is to have the following calls:
// 1. CM: controller.get_node( )->configure( )
// 2. LifecycleNode: ControllerInterfaceBase::on_configure( )
// 3. ControllerInterfaceBase: <controller>::on_configure( )
// Then we don't need to do state-machine related checks.
//
// Other solution is to add check into the LifecycleNode if a transition is valid to trigger
if ( get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED )
{
update_rate_ = get_node( )->get_parameter( "update_rate" ).as_int( );
}
return get_node( )->configure( );
}
92 void ControllerInterfaceBase::assign_interfaces(
std::vector<hardware_interface::LoanedCommandInterface> && command_interfaces,
std::vector<hardware_interface::LoanedStateInterface> && state_interfaces )
{
command_interfaces_ = std::forward<decltype( command_interfaces )>( command_interfaces );
state_interfaces_ = std::forward<decltype( state_interfaces )>( state_interfaces );
}
100 void ControllerInterfaceBase::release_interfaces( )
{
command_interfaces_.clear( );
state_interfaces_.clear( );
}
106 const rclcpp_lifecycle::State & ControllerInterfaceBase::get_state( ) const
{
return node_->get_current_state( );
}
111 std::shared_ptr<rclcpp_lifecycle::LifecycleNode> ControllerInterfaceBase::get_node( )
{
if ( !node_.get( ) )
{
throw std::runtime_error( "Lifecycle node hasn't been initialized yet!" );
}
return node_;
}
120 std::shared_ptr<rclcpp_lifecycle::LifecycleNode> ControllerInterfaceBase::get_node( ) const
{
if ( !node_.get( ) )
{
throw std::runtime_error( "Lifecycle node hasn't been initialized yet!" );
}
return node_;
}
129 unsigned int ControllerInterfaceBase::get_update_rate( ) const { return update_rate_; }
} // namespace controller_interface
1 // Copyright ( c ) 2022, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "test_chainable_controller_interface.hpp"
#include <gmock/gmock.h>
19 TEST_F( ChainableControllerInterfaceTest, default_returns )
{
TestableChainableControllerInterface controller;
// initialize, create node
ASSERT_EQ( controller.init( TEST_CONTROLLER_NAME ), controller_interface::return_type::OK );
ASSERT_NO_THROW( controller.get_node( ) );
EXPECT_TRUE( controller.is_chainable( ) );
EXPECT_FALSE( controller.is_in_chained_mode( ) );
}
31 TEST_F( ChainableControllerInterfaceTest, export_reference_interfaces )
{
TestableChainableControllerInterface controller;
// initialize, create node
ASSERT_EQ( controller.init( TEST_CONTROLLER_NAME ), controller_interface::return_type::OK );
ASSERT_NO_THROW( controller.get_node( ) );
auto reference_interfaces = controller.export_reference_interfaces( );
ASSERT_EQ( reference_interfaces.size( ), 1u );
EXPECT_EQ( reference_interfaces[0].get_prefix_name( ), TEST_CONTROLLER_NAME );
EXPECT_EQ( reference_interfaces[0].get_interface_name( ), "test_itf" );
EXPECT_EQ( reference_interfaces[0].get_value( ), INTERFACE_VALUE );
}
48 TEST_F( ChainableControllerInterfaceTest, reference_interfaces_storage_not_correct_size )
{
TestableChainableControllerInterface controller;
// initialize, create node
ASSERT_EQ( controller.init( TEST_CONTROLLER_NAME ), controller_interface::return_type::OK );
ASSERT_NO_THROW( controller.get_node( ) );
// expect empty return because storage is not resized
controller.reference_interfaces_.clear( );
auto reference_interfaces = controller.export_reference_interfaces( );
ASSERT_TRUE( reference_interfaces.empty( ) );
}
62 TEST_F( ChainableControllerInterfaceTest, reference_interfaces_prefix_is_not_node_name )
{
TestableChainableControllerInterface controller;
// initialize, create node
ASSERT_EQ( controller.init( TEST_CONTROLLER_NAME ), controller_interface::return_type::OK );
ASSERT_NO_THROW( controller.get_node( ) );
controller.set_name_prefix_of_reference_interfaces( "some_not_correct_interface_prefix" );
// expect empty return because interface prefix is not equal to the node name
auto reference_interfaces = controller.export_reference_interfaces( );
ASSERT_TRUE( reference_interfaces.empty( ) );
}
77 TEST_F( ChainableControllerInterfaceTest, setting_chained_mode )
{
TestableChainableControllerInterface controller;
// initialize, create node
ASSERT_EQ( controller.init( TEST_CONTROLLER_NAME ), controller_interface::return_type::OK );
ASSERT_NO_THROW( controller.get_node( ) );
auto reference_interfaces = controller.export_reference_interfaces( );
ASSERT_EQ( reference_interfaces.size( ), 1u );
EXPECT_FALSE( controller.is_in_chained_mode( ) );
// Fail setting chained mode
EXPECT_EQ( reference_interfaces[0].get_value( ), INTERFACE_VALUE );
EXPECT_FALSE( controller.set_chained_mode( true ) );
EXPECT_FALSE( controller.is_in_chained_mode( ) );
EXPECT_FALSE( controller.set_chained_mode( false ) );
EXPECT_FALSE( controller.is_in_chained_mode( ) );
// Success setting chained mode
reference_interfaces[0].set_value( 0.0 );
EXPECT_TRUE( controller.set_chained_mode( true ) );
EXPECT_TRUE( controller.is_in_chained_mode( ) );
controller.configure( );
EXPECT_TRUE( controller.set_chained_mode( false ) );
EXPECT_FALSE( controller.is_in_chained_mode( ) );
controller.get_node( )->activate( );
// Can not change chained mode until in "ACTIVE" state
EXPECT_FALSE( controller.set_chained_mode( true ) );
EXPECT_FALSE( controller.is_in_chained_mode( ) );
controller.get_node( )->deactivate( );
EXPECT_TRUE( controller.set_chained_mode( true ) );
EXPECT_TRUE( controller.is_in_chained_mode( ) );
// Can change 'chained' mode only in "UNCONFIGURED" state
controller.get_node( )->cleanup( );
EXPECT_TRUE( controller.set_chained_mode( false ) );
EXPECT_FALSE( controller.is_in_chained_mode( ) );
}
124 TEST_F( ChainableControllerInterfaceTest, test_update_logic )
{
TestableChainableControllerInterface controller;
// initialize, create node
ASSERT_EQ( controller.init( TEST_CONTROLLER_NAME ), controller_interface::return_type::OK );
ASSERT_NO_THROW( controller.get_node( ) );
EXPECT_FALSE( controller.is_in_chained_mode( ) );
// call update and update it from subscriber because not in chained mode
ASSERT_EQ(
controller.update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
ASSERT_EQ( controller.reference_interfaces_[0], INTERFACE_VALUE_INITIAL_REF - 1 );
// Provoke error in update from subscribers - return ERROR and update_and_write_commands not exec.
controller.set_new_reference_interface_value( INTERFACE_VALUE_SUBSCRIBER_ERROR );
ASSERT_EQ(
controller.update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::ERROR );
ASSERT_EQ( controller.reference_interfaces_[0], INTERFACE_VALUE_INITIAL_REF - 1 );
// Provoke error from update - return ERROR, but reference interface is updated and not reduced
controller.set_new_reference_interface_value( INTERFACE_VALUE_UPDATE_ERROR );
ASSERT_EQ(
controller.update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::ERROR );
ASSERT_EQ( controller.reference_interfaces_[0], INTERFACE_VALUE_UPDATE_ERROR );
controller.reference_interfaces_[0] = 0.0;
EXPECT_TRUE( controller.set_chained_mode( true ) );
EXPECT_TRUE( controller.is_in_chained_mode( ) );
// Provoke error in update from subscribers - return OK because update of subscribers is not used
// reference interface is not updated ( updated directly because in chained mode )
controller.set_new_reference_interface_value( INTERFACE_VALUE_SUBSCRIBER_ERROR );
ASSERT_EQ(
controller.update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
ASSERT_EQ( controller.reference_interfaces_[0], -1.0 );
// Provoke error from update - return ERROR, but reference interface is updated directly
controller.set_new_reference_interface_value( INTERFACE_VALUE_SUBSCRIBER_ERROR );
controller.reference_interfaces_[0] = INTERFACE_VALUE_UPDATE_ERROR;
ASSERT_EQ(
controller.update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::ERROR );
ASSERT_EQ( controller.reference_interfaces_[0], INTERFACE_VALUE_UPDATE_ERROR );
}
1 // Copyright ( c ) 2022, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TEST_CHAINABLE_CONTROLLER_INTERFACE_HPP_
#define TEST_CHAINABLE_CONTROLLER_INTERFACE_HPP_
#include <string>
#include <vector>
#include "gmock/gmock.h"
#include "controller_interface/chainable_controller_interface.hpp"
#include "hardware_interface/handle.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
27 constexpr char TEST_CONTROLLER_NAME[] = "testable_chainable_controller";
28 constexpr double INTERFACE_VALUE = 1989.0;
constexpr double INTERFACE_VALUE_SUBSCRIBER_ERROR = 12345.0;
30 constexpr double INTERFACE_VALUE_UPDATE_ERROR = 67890.0;
31 constexpr double INTERFACE_VALUE_INITIAL_REF = 1984.0;
33 class TestableChainableControllerInterface
: public controller_interface::ChainableControllerInterface
{
public:
FRIEND_TEST( ChainableControllerInterfaceTest, reference_interfaces_storage_not_correct_size );
FRIEND_TEST( ChainableControllerInterfaceTest, test_update_logic );
TestableChainableControllerInterface( )
{
reference_interfaces_.reserve( 1 );
reference_interfaces_.push_back( INTERFACE_VALUE );
}
controller_interface::CallbackReturn on_init( ) override
{
// set default value
name_prefix_of_reference_interfaces_ = get_node( )->get_name( );
return controller_interface::CallbackReturn::SUCCESS;
}
controller_interface::InterfaceConfiguration command_interface_configuration( ) const override
{
return controller_interface::InterfaceConfiguration{
controller_interface::interface_configuration_type::NONE};
}
controller_interface::InterfaceConfiguration state_interface_configuration( ) const override
{
return controller_interface::InterfaceConfiguration{
controller_interface::interface_configuration_type::NONE};
}
// Implementation of ChainableController virtual methods
std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces( ) override
{
std::vector<hardware_interface::CommandInterface> command_interfaces;
command_interfaces.push_back( hardware_interface::CommandInterface(
name_prefix_of_reference_interfaces_, "test_itf", &reference_interfaces_[0] ) );
return command_interfaces;
}
bool on_set_chained_mode( bool /*chained_mode*/ ) override
{
if ( reference_interfaces_[0] == 0.0 )
{
return true;
}
else
{
return false;
}
}
controller_interface::return_type update_reference_from_subscribers( ) override
{
if ( reference_interface_value_ == INTERFACE_VALUE_SUBSCRIBER_ERROR )
{
return controller_interface::return_type::ERROR;
}
reference_interfaces_[0] = reference_interface_value_;
return controller_interface::return_type::OK;
}
controller_interface::return_type update_and_write_commands(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) override
{
if ( reference_interfaces_[0] == INTERFACE_VALUE_UPDATE_ERROR )
{
return controller_interface::return_type::ERROR;
}
reference_interfaces_[0] -= 1;
return controller_interface::return_type::OK;
}
void set_name_prefix_of_reference_interfaces( const std::string & prefix )
{
name_prefix_of_reference_interfaces_ = prefix;
}
void set_new_reference_interface_value( const double ref_itf_value )
{
reference_interface_value_ = ref_itf_value;
}
std::string name_prefix_of_reference_interfaces_;
double reference_interface_value_ = INTERFACE_VALUE_INITIAL_REF;
};
127 class ChainableControllerInterfaceTest : public ::testing::Test
{
public:
130 static void SetUpTestCase( ) { rclcpp::init( 0, nullptr ); }
132 static void TearDownTestCase( ) { rclcpp::shutdown( ); }
};
#endif // TEST_CHAINABLE_CONTROLLER_INTERFACE_HPP_
// Copyright ( c ) 2022, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TEST_CONTROLLER_INTERFACE_HPP_
#define TEST_CONTROLLER_INTERFACE_HPP_
#include "controller_interface/controller_interface.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
21 constexpr char TEST_CONTROLLER_NAME[] = "testable_controller_interface";
23 class TestableControllerInterface : public controller_interface::ControllerInterface
{
public:
26 controller_interface::CallbackReturn on_init( ) override
{
return controller_interface::CallbackReturn::SUCCESS;
}
31 controller_interface::InterfaceConfiguration command_interface_configuration( ) const override
{
return controller_interface::InterfaceConfiguration{
controller_interface::interface_configuration_type::NONE};
}
controller_interface::InterfaceConfiguration state_interface_configuration( ) const override
{
return controller_interface::InterfaceConfiguration{
controller_interface::interface_configuration_type::NONE};
}
controller_interface::return_type update(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) override
{
return controller_interface::return_type::OK;
}
};
class TestableControllerInterfaceInitError : public TestableControllerInterface
{
public:
controller_interface::CallbackReturn on_init( ) override
{
return controller_interface::CallbackReturn::ERROR;
}
};
class TestableControllerInterfaceInitFailure : public TestableControllerInterface
{
public:
controller_interface::CallbackReturn on_init( ) override
{
return controller_interface::CallbackReturn::FAILURE;
}
};
#endif // TEST_CONTROLLER_INTERFACE_HPP_
// Copyright 2021 ros2_control development team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "test_controller_with_options.hpp"
#include <gtest/gtest.h>
#include <string>
#include "rclcpp/rclcpp.hpp"
22 class FriendControllerWithOptions : public controller_with_options::ControllerWithOptions
{
24 FRIEND_TEST( ControllerWithOption, init_with_overrides );
25 FRIEND_TEST( ControllerWithOption, init_without_overrides );
};
template <class T, size_t N>
constexpr size_t arrlen( T ( & )[N] )
{
return N;
}
TEST( ControllerWithOption, init_with_overrides )
{
// mocks the declaration of overrides parameters in a yaml file
char const * const argv[] = {"", "--ros-args",
"-p", "parameter_list.parameter1:=1.",
"-p", "parameter_list.parameter2:=2.",
"-p", "parameter_list.parameter3:=3."};
int argc = arrlen( argv );
rclcpp::init( argc, argv );
// creates the controller
FriendControllerWithOptions controller;
EXPECT_EQ( controller.init( "controller_name" ), controller_interface::return_type::OK );
// checks that the node options have been updated
const auto & node_options = controller.get_node( )->get_node_options( );
EXPECT_TRUE( node_options.allow_undeclared_parameters( ) );
EXPECT_TRUE( node_options.automatically_declare_parameters_from_overrides( ) );
// checks that the parameters have been correctly processed
EXPECT_EQ( controller.params.size( ), 3u );
EXPECT_EQ( controller.params["parameter1"], 1. );
EXPECT_EQ( controller.params["parameter2"], 2. );
EXPECT_EQ( controller.params["parameter3"], 3. );
rclcpp::shutdown( );
}
TEST( ControllerWithOption, init_without_overrides )
{
// mocks the declaration of overrides parameters in a yaml file
char const * const argv[] = {""};
int argc = arrlen( argv );
rclcpp::init( argc, argv );
// creates the controller
FriendControllerWithOptions controller;
EXPECT_EQ( controller.init( "controller_name" ), controller_interface::return_type::ERROR );
// checks that the node options have been updated
const auto & node_options = controller.get_node( )->get_node_options( );
EXPECT_TRUE( node_options.allow_undeclared_parameters( ) );
EXPECT_TRUE( node_options.automatically_declare_parameters_from_overrides( ) );
// checks that no parameter has been declared from overrides
EXPECT_EQ( controller.params.size( ), 0u );
rclcpp::shutdown( );
}
// Copyright 2021 ros2_control development team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TEST_CONTROLLER_WITH_OPTIONS_HPP_
#define TEST_CONTROLLER_WITH_OPTIONS_HPP_
#include <map>
#include <memory>
#include <string>
#include "controller_interface/controller_interface.hpp"
#include "hardware_interface/types/lifecycle_state_names.hpp"
namespace controller_with_options
{
/**
* Example of Controller using the ControllerInterface::init( const std::string &,
* rclcpp::NodeOptions & ) function. In this example, we set the node options so that parameters
* are automatically declared by overrides. This is a rare use case, but it can be useful in some
* situations.
*/
33 class ControllerWithOptions : public controller_interface::ControllerInterface
{
public:
ControllerWithOptions( ) = default;
LifecycleNodeInterface::CallbackReturn on_init( ) override
{
return LifecycleNodeInterface::CallbackReturn::SUCCESS;
}
42 controller_interface::return_type init(
const std::string & controller_name, const std::string & namespace_ = "",
const rclcpp::NodeOptions & node_options =
rclcpp::NodeOptions( )
.allow_undeclared_parameters( true )
.automatically_declare_parameters_from_overrides( true ) ) override
{
ControllerInterface::init( controller_name, namespace_, node_options );
switch ( on_init( ) )
{
case LifecycleNodeInterface::CallbackReturn::SUCCESS:
break;
case LifecycleNodeInterface::CallbackReturn::ERROR:
case LifecycleNodeInterface::CallbackReturn::FAILURE:
return controller_interface::return_type::ERROR;
}
if ( get_node( )->get_parameters( "parameter_list", params ) )
{
RCLCPP_INFO_STREAM( get_node( )->get_logger( ), "I found " << params.size( ) << " parameters." );
return controller_interface::return_type::OK;
}
else
{
return controller_interface::return_type::ERROR;
}
}
70 controller_interface::InterfaceConfiguration command_interface_configuration( ) const override
{
return controller_interface::InterfaceConfiguration{
controller_interface::interface_configuration_type::NONE};
}
76 controller_interface::InterfaceConfiguration state_interface_configuration( ) const override
{
return controller_interface::InterfaceConfiguration{
controller_interface::interface_configuration_type::NONE};
}
controller_interface::return_type update(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) override
{
return controller_interface::return_type::OK;
}
std::map<std::string, double> params;
};
} // namespace controller_with_options
#endif // TEST_CONTROLLER_WITH_OPTIONS_HPP_
1 // Copyright ( c ) 2021, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*
* Authors: Subhas Das, Denis Stogl
*/
#include "test_force_torque_sensor.hpp"
#include <cmath>
#include <memory>
#include <string>
#include <vector>
26 void ForceTorqueSensorTest::TearDown( ) { force_torque_sensor_.reset( nullptr ); }
28 TEST_F( ForceTorqueSensorTest, validate_all_with_default_names )
{
// create the force torque sensor
force_torque_sensor_ = std::make_unique<TestableForceTorqueSensor>( sensor_name_ );
// validate the component name
ASSERT_EQ( force_torque_sensor_->name_, sensor_name_ );
// validate the space reserved for interface_names_ and state_interfaces_
// Note : Using capacity( ) for state_interfaces_ as no such interfaces are defined yet
ASSERT_EQ( force_torque_sensor_->interface_names_.size( ), size_ );
ASSERT_EQ( force_torque_sensor_->state_interfaces_.capacity( ), size_ );
// validate the default interface_names_
ASSERT_TRUE( std::equal(
force_torque_sensor_->interface_names_.begin( ), force_torque_sensor_->interface_names_.end( ),
full_interface_names_.begin( ), full_interface_names_.end( ) ) );
// get the interface names
std::vector<std::string> interface_names = force_torque_sensor_->get_state_interface_names( );
// assign values to force
hardware_interface::StateInterface force_x{
sensor_name_, fts_interface_names_[0], &force_values_[0]};
hardware_interface::StateInterface force_y{
sensor_name_, fts_interface_names_[1], &force_values_[1]};
hardware_interface::StateInterface force_z{
sensor_name_, fts_interface_names_[2], &force_values_[2]};
// assign values to torque
hardware_interface::StateInterface torque_x{
sensor_name_, fts_interface_names_[3], &torque_values_[0]};
hardware_interface::StateInterface torque_y{
sensor_name_, fts_interface_names_[4], &torque_values_[1]};
hardware_interface::StateInterface torque_z{
sensor_name_, fts_interface_names_[5], &torque_values_[2]};
// create local state interface vector
std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;
temp_state_interfaces.reserve( 6 );
// insert the interfaces in jumbled sequence
temp_state_interfaces.emplace_back( torque_y );
temp_state_interfaces.emplace_back( force_z );
temp_state_interfaces.emplace_back( force_x );
temp_state_interfaces.emplace_back( torque_z );
temp_state_interfaces.emplace_back( torque_x );
temp_state_interfaces.emplace_back( force_y );
// now call the function to make them in order like interface_names
force_torque_sensor_->assign_loaned_state_interfaces( temp_state_interfaces );
// validate the count of state_interfaces_
ASSERT_EQ( force_torque_sensor_->state_interfaces_.size( ), size_ );
// validate the force values
std::array<double, 3> temp_force_values = force_torque_sensor_->get_forces( );
ASSERT_EQ( temp_force_values, force_values_ );
// validate the torque values
std::array<double, 3> temp_torque_values = force_torque_sensor_->get_torques( );
ASSERT_EQ( temp_torque_values, torque_values_ );
// validate get_values_as_message
geometry_msgs::msg::Wrench temp_message;
ASSERT_TRUE( force_torque_sensor_->get_values_as_message( temp_message ) );
ASSERT_EQ( temp_message.force.x, force_values_[0] );
ASSERT_EQ( temp_message.force.y, force_values_[1] );
ASSERT_EQ( temp_message.force.z, force_values_[2] );
ASSERT_EQ( temp_message.torque.x, torque_values_[0] );
ASSERT_EQ( temp_message.torque.y, torque_values_[1] );
ASSERT_EQ( temp_message.torque.z, torque_values_[2] );
// release the state_interfaces_
force_torque_sensor_->release_interfaces( );
// validate the count of state_interfaces_
ASSERT_EQ( force_torque_sensor_->state_interfaces_.size( ), 0u );
}
108 TEST_F( ForceTorqueSensorTest, validate_all_with_custom_names )
{
// create the force torque sensor with force.x, force.z and torque.y, torque.z
force_torque_sensor_ = std::make_unique<TestableForceTorqueSensor>(
sensor_name_ + "/" + "force.x", "", sensor_name_ + "/" + "force.z", "",
sensor_name_ + "/" + "torque.y", sensor_name_ + "/" + "torque.z" );
// validate the component name
// it should be "" as we specified the interface's names explicitly
ASSERT_EQ( force_torque_sensor_->name_, "" );
// validate the space reserved for interface_names_ and state_interfaces_
// Note : Using capacity( ) for state_interfaces_ as no such interfaces are defined yet
ASSERT_EQ( force_torque_sensor_->interface_names_.size( ), 4u );
ASSERT_EQ( force_torque_sensor_->state_interfaces_.capacity( ), size_ );
// validate the default interface_names_
ASSERT_EQ( force_torque_sensor_->interface_names_[0], full_interface_names_[0] );
ASSERT_EQ( force_torque_sensor_->interface_names_[1], full_interface_names_[2] );
ASSERT_EQ( force_torque_sensor_->interface_names_[2], full_interface_names_[4] );
ASSERT_EQ( force_torque_sensor_->interface_names_[3], full_interface_names_[5] );
// get the interface names
std::vector<std::string> interface_names = force_torque_sensor_->get_state_interface_names( );
// assign values to force.x and force.z
hardware_interface::StateInterface force_x{
sensor_name_, fts_interface_names_[0], &force_values_[0]};
hardware_interface::StateInterface force_z{
sensor_name_, fts_interface_names_[2], &force_values_[2]};
// assign values to torque.y and torque.z
hardware_interface::StateInterface torque_y{
sensor_name_, fts_interface_names_[4], &torque_values_[1]};
hardware_interface::StateInterface torque_z{
sensor_name_, fts_interface_names_[5], &torque_values_[2]};
// create local state interface vector
std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;
temp_state_interfaces.reserve( 4 );
// insert the interfaces in jumbled sequence
temp_state_interfaces.emplace_back( torque_y );
temp_state_interfaces.emplace_back( force_z );
temp_state_interfaces.emplace_back( force_x );
temp_state_interfaces.emplace_back( torque_z );
// now call the function to make them in order like interface_names
force_torque_sensor_->assign_loaned_state_interfaces( temp_state_interfaces );
// validate the count of state_interfaces_
ASSERT_EQ( force_torque_sensor_->state_interfaces_.size( ), 4u );
// validate the force values
std::array<double, 3> temp_force_values = force_torque_sensor_->get_forces( );
ASSERT_EQ( temp_force_values[0], force_values_[0] );
ASSERT_TRUE( std::isnan( temp_force_values[1] ) );
ASSERT_EQ( temp_force_values[2], force_values_[2] );
// validate the torque values
std::array<double, 3> temp_torque_values = force_torque_sensor_->get_torques( );
ASSERT_TRUE( std::isnan( temp_torque_values[0] ) );
ASSERT_EQ( temp_torque_values[1], torque_values_[1] );
ASSERT_EQ( temp_torque_values[2], torque_values_[2] );
// validate get_values_as_message
geometry_msgs::msg::Wrench temp_message;
ASSERT_TRUE( force_torque_sensor_->get_values_as_message( temp_message ) );
ASSERT_EQ( temp_message.force.x, force_values_[0] );
ASSERT_TRUE( std::isnan( temp_message.force.y ) );
ASSERT_EQ( temp_message.force.z, force_values_[2] );
ASSERT_TRUE( std::isnan( temp_message.torque.x ) );
ASSERT_EQ( temp_message.torque.y, torque_values_[1] );
ASSERT_EQ( temp_message.torque.z, torque_values_[2] );
// release the state_interfaces_
force_torque_sensor_->release_interfaces( );
// validate the count of state_interfaces_
ASSERT_EQ( force_torque_sensor_->state_interfaces_.size( ), 0u );
}
190 TEST_F( ForceTorqueSensorTest, validate_all_custom_names )
{
// create the force torque sensor with force.x, force.z and torque.y, torque.z
force_torque_sensor_ = std::make_unique<TestableForceTorqueSensor>(
sensor_name_ + "/" + "force.x", sensor_name_ + "/" + "force.y", sensor_name_ + "/" + "force.z",
sensor_name_ + "/" + "torque.x", sensor_name_ + "/" + "torque.y",
sensor_name_ + "/" + "torque.z" );
// validate the component name
// it should be "" as we specified the interface's names explicitly
ASSERT_EQ( force_torque_sensor_->name_, "" );
// validate the space reserved for interface_names_ and state_interfaces_
// Note : Using capacity( ) for state_interfaces_ as no such interfaces are defined yet
ASSERT_EQ( force_torque_sensor_->interface_names_.size( ), size_ );
ASSERT_EQ( force_torque_sensor_->state_interfaces_.capacity( ), size_ );
// validate the custom interface_names_
ASSERT_EQ( force_torque_sensor_->interface_names_[0], sensor_name_ + "/" + "force.x" );
ASSERT_EQ( force_torque_sensor_->interface_names_[1], sensor_name_ + "/" + "force.y" );
ASSERT_EQ( force_torque_sensor_->interface_names_[2], sensor_name_ + "/" + "force.z" );
ASSERT_EQ( force_torque_sensor_->interface_names_[3], sensor_name_ + "/" + "torque.x" );
ASSERT_EQ( force_torque_sensor_->interface_names_[4], sensor_name_ + "/" + "torque.y" );
ASSERT_EQ( force_torque_sensor_->interface_names_[5], sensor_name_ + "/" + "torque.z" );
// assign values to force
hardware_interface::StateInterface force_x{
sensor_name_, fts_interface_names_[0], &force_values_[0]};
hardware_interface::StateInterface force_y{
sensor_name_, fts_interface_names_[1], &force_values_[1]};
hardware_interface::StateInterface force_z{
sensor_name_, fts_interface_names_[2], &force_values_[2]};
// assign values to torque
hardware_interface::StateInterface torque_x{
sensor_name_, fts_interface_names_[3], &torque_values_[0]};
hardware_interface::StateInterface torque_y{
sensor_name_, fts_interface_names_[4], &torque_values_[1]};
hardware_interface::StateInterface torque_z{
sensor_name_, fts_interface_names_[5], &torque_values_[2]};
// create local state interface vector
std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;
// insert the interfaces in jumbled sequence
temp_state_interfaces.emplace_back( torque_y );
temp_state_interfaces.emplace_back( force_z );
temp_state_interfaces.emplace_back( force_x );
temp_state_interfaces.emplace_back( torque_z );
temp_state_interfaces.emplace_back( torque_x );
temp_state_interfaces.emplace_back( force_y );
// now call the function to make them in order like interface_names
force_torque_sensor_->assign_loaned_state_interfaces( temp_state_interfaces );
// validate the count of state_interfaces_
ASSERT_EQ( force_torque_sensor_->state_interfaces_.size( ), size_ );
// validate the force values
std::array<double, 3> temp_force_values = force_torque_sensor_->get_forces( );
ASSERT_EQ( temp_force_values[0], force_values_[0] );
ASSERT_EQ( temp_force_values[1], force_values_[1] );
ASSERT_EQ( temp_force_values[2], force_values_[2] );
// validate the torque values
std::array<double, 3> temp_torque_values = force_torque_sensor_->get_torques( );
ASSERT_EQ( temp_torque_values[0], torque_values_[0] );
ASSERT_EQ( temp_torque_values[1], torque_values_[1] );
ASSERT_EQ( temp_torque_values[2], torque_values_[2] );
// validate get_values_as_message
geometry_msgs::msg::Wrench temp_message;
ASSERT_TRUE( force_torque_sensor_->get_values_as_message( temp_message ) );
ASSERT_EQ( temp_message.force.x, force_values_[0] );
ASSERT_EQ( temp_message.force.y, force_values_[1] );
ASSERT_EQ( temp_message.force.z, force_values_[2] );
ASSERT_EQ( temp_message.torque.x, torque_values_[0] );
ASSERT_EQ( temp_message.torque.y, torque_values_[1] );
ASSERT_EQ( temp_message.torque.z, torque_values_[2] );
// release the state_interfaces_
force_torque_sensor_->release_interfaces( );
// validate the count of state_interfaces_
ASSERT_EQ( force_torque_sensor_->state_interfaces_.size( ), 0u );
}
1 // Copyright ( c ) 2021, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*
* Authors: Subhas Das, Denis Stogl
*/
#ifndef TEST_FORCE_TORQUE_SENSOR_HPP_
#define TEST_FORCE_TORQUE_SENSOR_HPP_
#include <memory>
#include <string>
#include <vector>
#include "gmock/gmock.h"
#include "semantic_components/force_torque_sensor.hpp"
// implementing and friending so we can access member variables
31 class TestableForceTorqueSensor : public semantic_components::ForceTorqueSensor
{
33 FRIEND_TEST( ForceTorqueSensorTest, validate_all_with_default_names );
34 FRIEND_TEST( ForceTorqueSensorTest, validate_all_with_custom_names );
35 FRIEND_TEST( ForceTorqueSensorTest, validate_all_custom_names );
public:
// Use generation of interface names
39 explicit TestableForceTorqueSensor( const std::string & name ) : ForceTorqueSensor( name ) {}
// Use custom interface names
41 explicit TestableForceTorqueSensor(
42 const std::string & interface_force_x, const std::string & interface_force_y,
43 const std::string & interface_force_z, const std::string & interface_torque_x,
44 const std::string & interface_torque_y, const std::string & interface_torque_z )
: ForceTorqueSensor(
interface_force_x, interface_force_y, interface_force_z, interface_torque_x,
interface_torque_y, interface_torque_z )
{
}
virtual ~TestableForceTorqueSensor( ) = default;
};
54 class ForceTorqueSensorTest : public ::testing::Test
{
public:
57 void SetUp( )
{
full_interface_names_.reserve( size_ );
for ( auto index = 0u; index < size_; ++index )
{
full_interface_names_.emplace_back( sensor_name_ + "/" + fts_interface_names_[index] );
}
}
66 void TearDown( );
protected:
69 const size_t size_ = 6;
70 const std::string sensor_name_ = "test_FTS";
71 std::array<double, 3> force_values_ = {1.1, 2.2, 3.3};
72 std::array<double, 3> torque_values_ = {4.4, 5.5, 6.6};
73 std::unique_ptr<TestableForceTorqueSensor> force_torque_sensor_;
75 std::vector<std::string> full_interface_names_;
76 const std::vector<std::string> fts_interface_names_ = {"force.x", "force.y", "force.z",
"torque.x", "torque.y", "torque.z"};
};
#endif // TEST_FORCE_TORQUE_SENSOR_HPP_
1 // Copyright 2021 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*
* Authors: Subhas Das, Denis Stogl, Victor Lopez
*/
#include "test_imu_sensor.hpp"
#include <memory>
#include <string>
#include <vector>
25 void IMUSensorTest::TearDown( ) { imu_sensor_.reset( nullptr ); }
27 TEST_F( IMUSensorTest, validate_all )
{
// create the IMU sensor
imu_sensor_ = std::make_unique<TestableIMUSensor>( sensor_name_ );
// validate the component name
ASSERT_EQ( imu_sensor_->name_, sensor_name_ );
// validate the space reserved for interface_names_ and state_interfaces_
// Note : Using capacity( ) for state_interfaces_ as no such interfaces are defined yet
ASSERT_EQ( imu_sensor_->interface_names_.size( ), size_ );
ASSERT_EQ( imu_sensor_->state_interfaces_.capacity( ), size_ );
// validate the default interface_names_
ASSERT_TRUE( std::equal(
imu_sensor_->interface_names_.begin( ), imu_sensor_->interface_names_.end( ),
full_interface_names_.begin( ), full_interface_names_.end( ) ) );
// get the interface names
std::vector<std::string> interface_names = imu_sensor_->get_state_interface_names( );
// assign values to orientation
hardware_interface::StateInterface orientation_x{
sensor_name_, imu_interface_names_[0], &orientation_values_[0]};
hardware_interface::StateInterface orientation_y{
sensor_name_, imu_interface_names_[1], &orientation_values_[1]};
hardware_interface::StateInterface orientation_z{
sensor_name_, imu_interface_names_[2], &orientation_values_[2]};
hardware_interface::StateInterface orientation_w{
sensor_name_, imu_interface_names_[3], &orientation_values_[4]};
// assign values to angular velocity
hardware_interface::StateInterface angular_velocity_x{
sensor_name_, imu_interface_names_[4], &angular_velocity_values_[0]};
hardware_interface::StateInterface angular_velocity_y{
sensor_name_, imu_interface_names_[5], &angular_velocity_values_[1]};
hardware_interface::StateInterface angular_velocity_z{
sensor_name_, imu_interface_names_[6], &angular_velocity_values_[2]};
// assign values to linear acceleration
hardware_interface::StateInterface linear_acceleration_x{
sensor_name_, imu_interface_names_[7], &linear_acceleration_values_[0]};
hardware_interface::StateInterface linear_acceleration_y{
sensor_name_, imu_interface_names_[8], &linear_acceleration_values_[1]};
hardware_interface::StateInterface linear_acceleration_z{
sensor_name_, imu_interface_names_[9], &linear_acceleration_values_[2]};
// create local state interface vector
std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;
temp_state_interfaces.reserve( 10 );
// insert the interfaces in jumbled sequence
temp_state_interfaces.emplace_back( angular_velocity_y );
temp_state_interfaces.emplace_back( orientation_y );
temp_state_interfaces.emplace_back( linear_acceleration_y );
temp_state_interfaces.emplace_back( orientation_x );
temp_state_interfaces.emplace_back( linear_acceleration_z );
temp_state_interfaces.emplace_back( angular_velocity_z );
temp_state_interfaces.emplace_back( orientation_z );
temp_state_interfaces.emplace_back( orientation_w );
temp_state_interfaces.emplace_back( angular_velocity_x );
temp_state_interfaces.emplace_back( linear_acceleration_x );
// now call the function to make them in order like interface_names
imu_sensor_->assign_loaned_state_interfaces( temp_state_interfaces );
// validate the count of state_interfaces_
ASSERT_EQ( imu_sensor_->state_interfaces_.size( ), size_ );
// validate the orientation values
std::array<double, 4> temp_orientation_values = imu_sensor_->get_orientation( );
ASSERT_EQ( temp_orientation_values, orientation_values_ );
// validate the angular_velocity values
std::array<double, 3> temp_angular_velocity_values = imu_sensor_->get_angular_velocity( );
ASSERT_EQ( temp_angular_velocity_values, angular_velocity_values_ );
// validate the linear_acceleration values
std::array<double, 3> temp_linear_acceleration_values = imu_sensor_->get_linear_acceleration( );
ASSERT_EQ( temp_linear_acceleration_values, linear_acceleration_values_ );
// validate get_values_as_message
sensor_msgs::msg::Imu temp_message;
ASSERT_TRUE( imu_sensor_->get_values_as_message( temp_message ) );
ASSERT_EQ( temp_message.orientation.x, orientation_values_[0] );
ASSERT_EQ( temp_message.orientation.y, orientation_values_[1] );
ASSERT_EQ( temp_message.orientation.z, orientation_values_[2] );
ASSERT_EQ( temp_message.orientation.w, orientation_values_[3] );
ASSERT_EQ( temp_message.angular_velocity.x, angular_velocity_values_[0] );
ASSERT_EQ( temp_message.angular_velocity.y, angular_velocity_values_[1] );
ASSERT_EQ( temp_message.angular_velocity.z, angular_velocity_values_[2] );
ASSERT_EQ( temp_message.linear_acceleration.x, linear_acceleration_values_[0] );
ASSERT_EQ( temp_message.linear_acceleration.y, linear_acceleration_values_[1] );
ASSERT_EQ( temp_message.linear_acceleration.z, linear_acceleration_values_[2] );
// release the state_interfaces_
imu_sensor_->release_interfaces( );
// validate the count of state_interfaces_
ASSERT_EQ( imu_sensor_->state_interfaces_.size( ), 0u );
}
1 // Copyright 2021 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*
* Authors: Subhas Das, Denis Stogl, Victor Lopez
*/
#ifndef TEST_IMU_SENSOR_HPP_
#define TEST_IMU_SENSOR_HPP_
#include <memory>
#include <string>
#include <vector>
#include "gmock/gmock.h"
#include "semantic_components/imu_sensor.hpp"
// implementing and friending so we can access member variables
31 class TestableIMUSensor : public semantic_components::IMUSensor
{
33 FRIEND_TEST( IMUSensorTest, validate_all );
public:
// Use generation of interface names
37 explicit TestableIMUSensor( const std::string & name ) : IMUSensor( name ) {}
virtual ~TestableIMUSensor( ) = default;
};
42 class IMUSensorTest : public ::testing::Test
{
public:
45 void SetUp( )
{
full_interface_names_.reserve( size_ );
for ( auto index = 0u; index < size_; ++index )
{
full_interface_names_.emplace_back( sensor_name_ + "/" + imu_interface_names_[index] );
}
}
54 void TearDown( );
protected:
57 const size_t size_ = 10;
58 const std::string sensor_name_ = "test_IMU";
59 std::array<double, 4> orientation_values_ = {1.1, 2.2, 3.3, 4.4};
60 std::array<double, 3> angular_velocity_values_ = {4.4, 5.5, 6.6};
61 std::array<double, 3> linear_acceleration_values_ = {4.4, 5.5, 6.6};
62 std::unique_ptr<TestableIMUSensor> imu_sensor_;
64 std::vector<std::string> full_interface_names_;
65 const std::vector<std::string> imu_interface_names_ = {
"orientation.x", "orientation.y", "orientation.z", "orientation.w",
"angular_velocity.x", "angular_velocity.y", "angular_velocity.z", "linear_acceleration.x",
"linear_acceleration.y", "linear_acceleration.z"};
};
#endif // TEST_IMU_SENSOR_HPP_
1 // Copyright ( c ) 2021, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*
* Authors: Subhas Das, Denis Stogl
*/
#include "test_semantic_component_interface.hpp"
#include <memory>
#include <string>
#include <vector>
25 void SemanticComponentInterfaceTest::TearDown( ) { semantic_component_.reset( nullptr ); }
27 TEST_F( SemanticComponentInterfaceTest, validate_default_names )
{
// create 'test_component' with 5 interfaces using default naming
// e.g. test_component_1, test_component_2 so on...
semantic_component_ =
std::make_unique<TestableSemanticComponentInterface>( component_name_, size_ );
// validate the component name
ASSERT_EQ( semantic_component_->name_, component_name_ );
// validate the space reserved for interface_names_ and state_interfaces_
// Note : Using capacity( ) for state_interfaces_ as no such interfaces are defined yet
ASSERT_EQ( semantic_component_->interface_names_.capacity( ), size_ );
ASSERT_EQ( semantic_component_->state_interfaces_.capacity( ), size_ );
// validate the interface_names_
std::vector<std::string> interface_names = semantic_component_->get_state_interface_names( );
ASSERT_EQ( interface_names, semantic_component_->interface_names_ );
ASSERT_EQ( interface_names.size( ), size_ );
ASSERT_EQ( interface_names[0], component_name_ + "/1" );
ASSERT_EQ( interface_names[1], component_name_ + "/2" );
ASSERT_EQ( interface_names[2], component_name_ + "/3" );
ASSERT_EQ( interface_names[3], component_name_ + "/4" );
ASSERT_EQ( interface_names[4], component_name_ + "/5" );
}
54 TEST_F( SemanticComponentInterfaceTest, validate_custom_names )
{
// create a component with 5 interfaces using custom naming
// as defined in the constructor
semantic_component_ = std::make_unique<TestableSemanticComponentInterface>( size_ );
// validate the component name
ASSERT_EQ( semantic_component_->name_, semantic_component_->test_name_ );
// validate the space reserved for interface_names_ and state_interfaces_
// Note : Using capacity( ) for state_interfaces_ as no such interfaces are defined yet
ASSERT_EQ( semantic_component_->interface_names_.capacity( ), size_ );
ASSERT_EQ( semantic_component_->state_interfaces_.capacity( ), size_ );
// validate the interface_names_
std::vector<std::string> interface_names = semantic_component_->get_state_interface_names( );
ASSERT_TRUE( std::equal(
semantic_component_->interface_names_.begin( ), semantic_component_->interface_names_.end( ),
interface_names.begin( ), interface_names.end( ) ) );
ASSERT_EQ( interface_names.size( ), size_ );
ASSERT_EQ( interface_names[0], semantic_component_->test_name_ + "/i5" );
ASSERT_EQ( interface_names[1], semantic_component_->test_name_ + "/i6" );
ASSERT_EQ( interface_names[2], semantic_component_->test_name_ + "/i7" );
ASSERT_EQ( interface_names[3], semantic_component_->test_name_ + "/i8" );
ASSERT_EQ( interface_names[4], semantic_component_->test_name_ + "/i9" );
}
82 TEST_F( SemanticComponentInterfaceTest, validate_state_interfaces )
{
// create 'test_component' with 5 interfaces using default naming
// e.g. test_component_1, test_component_2 so on...
semantic_component_ =
std::make_unique<TestableSemanticComponentInterface>( component_name_, size_ );
// generate the interface_names_
std::vector<std::string> interface_names = semantic_component_->get_state_interface_names( );
// validate assign_loaned_state_interfaces
// create interfaces and assign values to it
std::vector<double> interface_values = {1.1, 3.3, 5.5};
// assign 1.1 to interface_1, 3.3 to interface_3 and 5.5 to interface_5
hardware_interface::StateInterface interface_1{component_name_, "1", &interface_values[0]};
hardware_interface::StateInterface interface_3{component_name_, "3", &interface_values[1]};
hardware_interface::StateInterface interface_5{component_name_, "5", &interface_values[2]};
// create local state interface vector
std::vector<hardware_interface::LoanedStateInterface> temp_state_interfaces;
// insert the interfaces in jumbled sequence
temp_state_interfaces.emplace_back( interface_5 );
temp_state_interfaces.emplace_back( interface_1 );
temp_state_interfaces.emplace_back( interface_3 );
// now call the function to make them in order like interface_names
semantic_component_->assign_loaned_state_interfaces( temp_state_interfaces );
// validate the count of state_interfaces_
ASSERT_EQ( semantic_component_->state_interfaces_.size( ), 3u );
// validate the values of state_interfaces_ which should be
// in order as per interface_names_
std::vector<double> temp_values;
ASSERT_FALSE( semantic_component_->get_values( temp_values ) );
// reserve memory for the vector and call get_values
temp_values.reserve( 3 );
ASSERT_TRUE( semantic_component_->get_values( temp_values ) );
ASSERT_EQ( temp_values, interface_values );
// release the state_interfaces_
semantic_component_->release_interfaces( );
// validate the count of state_interfaces_
ASSERT_EQ( semantic_component_->state_interfaces_.size( ), 0u );
// validate that release_interfaces( ) does not touch interface_names_
ASSERT_TRUE( std::equal(
semantic_component_->interface_names_.begin( ), semantic_component_->interface_names_.end( ),
interface_names.begin( ), interface_names.end( ) ) );
}
1 // Copyright ( c ) 2021, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*
* Authors: Subhas Das, Denis Stogl
*/
#ifndef TEST_SEMANTIC_COMPONENT_INTERFACE_HPP_
#define TEST_SEMANTIC_COMPONENT_INTERFACE_HPP_
#include <memory>
#include <string>
#include "geometry_msgs/msg/wrench.hpp"
#include "gmock/gmock.h"
#include "semantic_components/semantic_component_interface.hpp"
// implementing and friending so we can access member variables
30 class TestableSemanticComponentInterface
31 : public semantic_components::SemanticComponentInterface<geometry_msgs::msg::Wrench>
{
33 FRIEND_TEST( SemanticComponentInterfaceTest, validate_default_names );
34 FRIEND_TEST( SemanticComponentInterfaceTest, validate_custom_names );
35 FRIEND_TEST( SemanticComponentInterfaceTest, validate_state_interfaces );
public:
// Use generation of interface names
39 explicit TestableSemanticComponentInterface( const std::string & name, size_t size )
: SemanticComponentInterface<geometry_msgs::msg::Wrench>( name, size )
{
}
// Use custom interface names
44 explicit TestableSemanticComponentInterface( size_t size )
: SemanticComponentInterface( "TestSemanticComponent", size )
{
// generate the interface_names_
for ( auto i = 0u; i < size; ++i )
{
interface_names_.emplace_back(
std::string( "TestSemanticComponent" ) + "/i" + std::to_string( i + 5 ) );
}
}
virtual ~TestableSemanticComponentInterface( ) = default;
std::string test_name_ = "TestSemanticComponent";
};
60 class SemanticComponentInterfaceTest : public ::testing::Test
{
public:
63 void TearDown( );
protected:
66 const std::string component_name_ = "test_component";
67 const size_t size_ = 5;
68 std::unique_ptr<TestableSemanticComponentInterface> semantic_component_;
};
#endif // TEST_SEMANTIC_COMPONENT_INTERFACE_HPP_
// Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_
#define CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_
#include <map>
#include <memory>
#include <string>
#include <tuple>
#include <unordered_map>
#include <vector>
#include "controller_interface/chainable_controller_interface.hpp"
#include "controller_interface/controller_interface.hpp"
#include "controller_interface/controller_interface_base.hpp"
#include "controller_manager/controller_spec.hpp"
#include "controller_manager/visibility_control.h"
#include "controller_manager_msgs/srv/configure_controller.hpp"
#include "controller_manager_msgs/srv/list_controller_types.hpp"
#include "controller_manager_msgs/srv/list_controllers.hpp"
#include "controller_manager_msgs/srv/list_hardware_components.hpp"
#include "controller_manager_msgs/srv/list_hardware_interfaces.hpp"
#include "controller_manager_msgs/srv/load_controller.hpp"
#include "controller_manager_msgs/srv/reload_controller_libraries.hpp"
#include "controller_manager_msgs/srv/set_hardware_component_state.hpp"
#include "controller_manager_msgs/srv/switch_controller.hpp"
#include "controller_manager_msgs/srv/unload_controller.hpp"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/resource_manager.hpp"
#include "pluginlib/class_loader.hpp"
#include "rclcpp/executor.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/node_interfaces/node_logging_interface.hpp"
#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
#include "rclcpp/parameter.hpp"
namespace controller_manager
{
using ControllersListIterator = std::vector<controller_manager::ControllerSpec>::const_iterator;
57 class ControllerManager : public rclcpp::Node
{
public:
static constexpr bool kWaitForAllResources = false;
static constexpr auto kInfiniteTimeout = 0;
CONTROLLER_MANAGER_PUBLIC
ControllerManager(
std::unique_ptr<hardware_interface::ResourceManager> resource_manager,
std::shared_ptr<rclcpp::Executor> executor,
const std::string & manager_node_name = "controller_manager",
const std::string & namespace_ = "" );
CONTROLLER_MANAGER_PUBLIC
ControllerManager(
std::shared_ptr<rclcpp::Executor> executor,
const std::string & manager_node_name = "controller_manager",
const std::string & namespace_ = "" );
CONTROLLER_MANAGER_PUBLIC
virtual ~ControllerManager( ) = default;
CONTROLLER_MANAGER_PUBLIC
void init_resource_manager( const std::string & robot_description );
CONTROLLER_MANAGER_PUBLIC
controller_interface::ControllerInterfaceBaseSharedPtr load_controller(
const std::string & controller_name, const std::string & controller_type );
/// load_controller loads a controller by name, the type must be defined in the parameter server.
/**
* \param[in] controller_name as a string.
* \return controller
* \see Documentation in controller_manager_msgs/LoadController.srv
*/
CONTROLLER_MANAGER_PUBLIC
controller_interface::ControllerInterfaceBaseSharedPtr load_controller(
const std::string & controller_name );
CONTROLLER_MANAGER_PUBLIC
controller_interface::return_type unload_controller( const std::string & controller_name );
CONTROLLER_MANAGER_PUBLIC
std::vector<ControllerSpec> get_loaded_controllers( ) const;
template <
typename T, typename std::enable_if<
std::is_convertible<T *, controller_interface::ControllerInterfaceBase *>::value,
T>::type * = nullptr>
controller_interface::ControllerInterfaceBaseSharedPtr add_controller(
std::shared_ptr<T> controller, const std::string & controller_name,
const std::string & controller_type )
{
ControllerSpec controller_spec;
controller_spec.c = controller;
controller_spec.info.name = controller_name;
controller_spec.info.type = controller_type;
return add_controller_impl( controller_spec );
}
/// configure_controller Configure controller by name calling their "configure" method.
/**
* \param[in] controller_name as a string.
* \return configure controller response
* \see Documentation in controller_manager_msgs/ConfigureController.srv
*/
CONTROLLER_MANAGER_PUBLIC
124 controller_interface::return_type configure_controller( const std::string & controller_name );
/// switch_controller Stops some controllers and start others.
/**
* \param[in] start_controllers is a list of controllers to start
* \param[in] stop_controllers is a list of controllers to stop
* \param[in] set level of strictness ( BEST_EFFORT or STRICT )
* \see Documentation in controller_manager_msgs/SwitchController.srv
*/
CONTROLLER_MANAGER_PUBLIC
controller_interface::return_type switch_controller(
const std::vector<std::string> & start_controllers,
const std::vector<std::string> & stop_controllers, int strictness,
bool activate_asap = kWaitForAllResources,
const rclcpp::Duration & timeout = rclcpp::Duration::from_nanoseconds( kInfiniteTimeout ) );
140 CONTROLLER_MANAGER_PUBLIC
void read( const rclcpp::Time & time, const rclcpp::Duration & period );
CONTROLLER_MANAGER_PUBLIC
controller_interface::return_type update(
const rclcpp::Time & time, const rclcpp::Duration & period );
147 CONTROLLER_MANAGER_PUBLIC
void write( const rclcpp::Time & time, const rclcpp::Duration & period );
/// Deterministic ( real-time safe ) callback group, e.g., update function.
/**
* Deterministic ( real-time safe ) callback group for the update function. Default behavior
* is read hardware, update controller and finally write new values to the hardware.
*/
// TODO( anyone ): Due to issues with the MutliThreadedExecutor, this control loop does not rely on
// the executor ( see issue #260 ).
// rclcpp::CallbackGroup::SharedPtr deterministic_callback_group_;
// Per controller update rate support
160 CONTROLLER_MANAGER_PUBLIC
unsigned int get_update_rate( ) const;
protected:
164 CONTROLLER_MANAGER_PUBLIC
void init_services( );
CONTROLLER_MANAGER_PUBLIC
controller_interface::ControllerInterfaceBaseSharedPtr add_controller_impl(
const ControllerSpec & controller );
171 CONTROLLER_MANAGER_PUBLIC
void manage_switch( );
174 CONTROLLER_MANAGER_PUBLIC
void deactivate_controllers( );
/**
* Switch chained mode for all the controllers with respect to the following cases:
* - a preceding controller is getting activated --> switch controller to chained mode;
* - all preceding controllers are deactivated --> switch controller from chained mode.
*
* \param[in] chained_mode_switch_list list of controller to switch chained mode.
* \param[in] to_chained_mode flag if controller should be switched *to* or *from* chained mode.
*/
CONTROLLER_MANAGER_PUBLIC
186 void switch_chained_mode(
const std::vector<std::string> & chained_mode_switch_list, bool to_chained_mode );
189 CONTROLLER_MANAGER_PUBLIC
void activate_controllers( );
192 CONTROLLER_MANAGER_PUBLIC
void activate_controllers_asap( );
CONTROLLER_MANAGER_PUBLIC
196 void list_controllers_srv_cb(
const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request> request,
std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response> response );
200 CONTROLLER_MANAGER_PUBLIC
void list_hardware_interfaces_srv_cb(
const std::shared_ptr<controller_manager_msgs::srv::ListHardwareInterfaces::Request> request,
std::shared_ptr<controller_manager_msgs::srv::ListHardwareInterfaces::Response> response );
205 CONTROLLER_MANAGER_PUBLIC
void load_controller_service_cb(
const std::shared_ptr<controller_manager_msgs::srv::LoadController::Request> request,
std::shared_ptr<controller_manager_msgs::srv::LoadController::Response> response );
CONTROLLER_MANAGER_PUBLIC
211 void configure_controller_service_cb(
const std::shared_ptr<controller_manager_msgs::srv::ConfigureController::Request> request,
std::shared_ptr<controller_manager_msgs::srv::ConfigureController::Response> response );
215 CONTROLLER_MANAGER_PUBLIC
void reload_controller_libraries_service_cb(
const std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Request> request,
std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Response> response );
220 CONTROLLER_MANAGER_PUBLIC
void switch_controller_service_cb(
const std::shared_ptr<controller_manager_msgs::srv::SwitchController::Request> request,
std::shared_ptr<controller_manager_msgs::srv::SwitchController::Response> response );
CONTROLLER_MANAGER_PUBLIC
226 void unload_controller_service_cb(
const std::shared_ptr<controller_manager_msgs::srv::UnloadController::Request> request,
std::shared_ptr<controller_manager_msgs::srv::UnloadController::Response> response );
230 CONTROLLER_MANAGER_PUBLIC
void list_controller_types_srv_cb(
const std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Request> request,
std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Response> response );
235 CONTROLLER_MANAGER_PUBLIC
void list_hardware_components_srv_cb(
const std::shared_ptr<controller_manager_msgs::srv::ListHardwareComponents::Request> request,
std::shared_ptr<controller_manager_msgs::srv::ListHardwareComponents::Response> response );
CONTROLLER_MANAGER_PUBLIC
241 void set_hardware_component_state_srv_cb(
const std::shared_ptr<controller_manager_msgs::srv::SetHardwareComponentState::Request> request,
std::shared_ptr<controller_manager_msgs::srv::SetHardwareComponentState::Response> response );
// Per controller update rate support
unsigned int update_loop_counter_ = 0;
unsigned int update_rate_ = 100;
248 std::vector<std::vector<std::string>> chained_controllers_configuration_;
std::unique_ptr<hardware_interface::ResourceManager> resource_manager_;
private:
std::vector<std::string> get_controller_names( );
/**
* Clear request lists used when switching controllers. The lists are shared between "callback" and
* "control loop" threads.
*/
void clear_requests( );
/**
* If a controller is deactivated all following controllers ( if any exist ) should be switched
* 'from' the chained mode.
*
* \param[in] controllers list with controllers.
*/
void propagate_deactivation_of_chained_mode( const std::vector<ControllerSpec> & controllers );
/// Check if all the following controllers will be in active state and in the chained mode
/// after controllers' switch.
/**
* Check recursively that all following controllers of the @controller_it
* - are already active,
* - will not be deactivated,
* - or will be activated.
* The following controllers are added to the request to switch in the chained mode or removed
* from the request to switch from the chained mode.
*
* For each controller the whole chain of following controllers is checked.
*
* NOTE: The automatically adding of following controller into starting list is not implemented
* yet.
*
* \param[in] controllers list with controllers.
* \param[in] strictness if value is equal "MANIPULATE_CONTROLLERS_CHAIN" then all following
* controllers will be automatically added to the activate request list if they are not in the
* deactivate request.
* \param[in] controller_it iterator to the controller for which the following controllers are
* checked.
*
* \returns return_type::OK if all following controllers pass the checks, otherwise
* return_type::ERROR.
*/
controller_interface::return_type check_following_controllers_for_activate(
const std::vector<ControllerSpec> & controllers, int strictness,
const ControllersListIterator controller_it );
/// Check if all the preceding controllers will be in inactive state after controllers' switch.
/**
* Check that all preceding controllers of the @controller_it
* - are inactive,
* - will be deactivated,
* - and will not be activated.
*
* NOTE: The automatically adding of preceding controllers into stopping list is not implemented
* yet.
*
* \param[in] controllers list with controllers.
* \param[in] strictness if value is equal "MANIPULATE_CONTROLLERS_CHAIN" then all preceding
* controllers will be automatically added to the deactivate request list.
* \param[in] controller_it iterator to the controller for which the preceding controllers are
* checked.
*
* \returns return_type::OK if all preceding controllers pass the checks, otherwise
* return_type::ERROR.
*/
controller_interface::return_type check_preceeding_controllers_for_deactivate(
const std::vector<ControllerSpec> & controllers, int strictness,
const ControllersListIterator controller_it );
std::shared_ptr<rclcpp::Executor> executor_;
std::shared_ptr<pluginlib::ClassLoader<controller_interface::ControllerInterface>> loader_;
std::shared_ptr<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>
chainable_loader_;
/// Best effort ( non real-time safe ) callback group, e.g., service callbacks.
/**
* Best effort ( non real-time safe ) callback group for callbacks that can possibly break
* real-time requirements, for example, service callbacks.
*/
rclcpp::CallbackGroup::SharedPtr best_effort_callback_group_;
/**
* The RTControllerListWrapper class wraps a double-buffered list of controllers
* to avoid needing to lock the real-time thread when switching controllers in
* the non-real-time thread.
*
* There's always an "updated" list and an "outdated" one
* There's always an "used by rt" list and an "unused by rt" list
*
* The updated state changes on the switch_updated_list( )
* The rt usage state changes on the update_and_get_used_by_rt_list( )
*/
class RTControllerListWrapper
{
// *INDENT-OFF*
public:
// *INDENT-ON*
/// update_and_get_used_by_rt_list Makes the "updated" list the "used by rt" list
/**
* \warning Should only be called by the RT thread, no one should modify the
* updated list while it's being used
* \return reference to the updated list
*/
std::vector<ControllerSpec> & update_and_get_used_by_rt_list( );
/**
* get_unused_list Waits until the "outdated" and "unused by rt"
* lists match and returns a reference to it
* This referenced list can be modified safely until switch_updated_controller_list( )
* is called, at this point the RT thread may start using it at any time
* \param[in] guard Guard needed to make sure the caller is the only one accessing the unused by rt list
*/
std::vector<ControllerSpec> & get_unused_list(
const std::lock_guard<std::recursive_mutex> & guard );
/// get_updated_list Returns a const reference to the most updated list.
/**
* \warning May or may not being used by the realtime thread, read-only reference for safety
* \param[in] guard Guard needed to make sure the caller is the only one accessing the unused by rt list
*/
const std::vector<ControllerSpec> & get_updated_list(
const std::lock_guard<std::recursive_mutex> & guard ) const;
/**
* switch_updated_list Switches the "updated" and "outdated" lists, and waits
* until the RT thread is using the new "updated" list.
* \param[in] guard Guard needed to make sure the caller is the only one accessing the unused by rt list
*/
void switch_updated_list( const std::lock_guard<std::recursive_mutex> & guard );
// Mutex protecting the controllers list
// must be acquired before using any list other than the "used by rt"
mutable std::recursive_mutex controllers_lock_;
// *INDENT-OFF*
private:
// *INDENT-ON*
/// get_other_list get the list not pointed by index
/**
* \param[in] index int
*/
int get_other_list( int index ) const;
void wait_until_rt_not_using(
int index, std::chrono::microseconds sleep_delay = std::chrono::microseconds( 200 ) ) const;
std::vector<ControllerSpec> controllers_lists_[2];
/// The index of the controller list with the most updated information
int updated_controllers_index_ = 0;
/// The index of the controllers list being used in the real-time thread.
int used_by_realtime_controllers_index_ = -1;
};
RTControllerListWrapper rt_controllers_wrapper_;
/// mutex copied from ROS1 Control, protects service callbacks
/// not needed if we're guaranteed that the callbacks don't come from multiple threads
std::mutex services_lock_;
rclcpp::Service<controller_manager_msgs::srv::ListControllers>::SharedPtr
list_controllers_service_;
rclcpp::Service<controller_manager_msgs::srv::ListControllerTypes>::SharedPtr
list_controller_types_service_;
rclcpp::Service<controller_manager_msgs::srv::LoadController>::SharedPtr load_controller_service_;
rclcpp::Service<controller_manager_msgs::srv::ConfigureController>::SharedPtr
configure_controller_service_;
rclcpp::Service<controller_manager_msgs::srv::ReloadControllerLibraries>::SharedPtr
reload_controller_libraries_service_;
rclcpp::Service<controller_manager_msgs::srv::SwitchController>::SharedPtr
switch_controller_service_;
rclcpp::Service<controller_manager_msgs::srv::UnloadController>::SharedPtr
unload_controller_service_;
rclcpp::Service<controller_manager_msgs::srv::ListHardwareComponents>::SharedPtr
list_hardware_components_service_;
rclcpp::Service<controller_manager_msgs::srv::ListHardwareInterfaces>::SharedPtr
list_hardware_interfaces_service_;
rclcpp::Service<controller_manager_msgs::srv::SetHardwareComponentState>::SharedPtr
set_hardware_component_state_service_;
std::vector<std::string> activate_request_, deactivate_request_;
std::vector<std::string> to_chained_mode_request_, from_chained_mode_request_;
std::vector<std::string> activate_command_interface_request_,
deactivate_command_interface_request_;
struct SwitchParams
{
bool do_switch = {false};
bool started = {false};
rclcpp::Time init_time = {rclcpp::Time::max( )};
// Switch options
int strictness = {0};
bool activate_asap = {false};
rclcpp::Duration timeout = rclcpp::Duration{0, 0};
};
SwitchParams switch_params_;
};
} // namespace controller_manager
#endif // CONTROLLER_MANAGER__CONTROLLER_MANAGER_HPP_
1 // Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*
* Author: Wim Meeussen
*/
#ifndef CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_
#define CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_
#include <map>
#include <string>
#include <vector>
#include "controller_interface/controller_interface.hpp"
#include "hardware_interface/controller_info.hpp"
namespace controller_manager
{
/// Controller Specification
/**
* This struct contains both a pointer to a given controller, \ref c, as well
* as information about the controller, \ref info.
*
*/
struct ControllerSpec
{
hardware_interface::ControllerInfo info;
controller_interface::ControllerInterfaceBaseSharedPtr c;
};
} // namespace controller_manager
#endif // CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_
1 // Copyright 2022 PickNik Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef CONTROLLER_MANAGER__REALTIME_HPP_
#define CONTROLLER_MANAGER__REALTIME_HPP_
namespace controller_manager
{
/**
* Detect if realtime kernel is present.
* \returns true if realtime kernel is detected
*/
24 bool has_realtime_kernel( );
/**
* Configure SCHED_FIFO thread priority for the thread that calls this function
* \param[in] priority the priority of this thread from 0-99
* \returns true if configuring scheduler succeeded
*/
31 bool configure_sched_fifo( int priority );
} // namespace controller_manager
#endif // CONTROLLER_MANAGER__REALTIME_HPP_
1 // Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef CONTROLLER_MANAGER__VISIBILITY_CONTROL_H_
#define CONTROLLER_MANAGER__VISIBILITY_CONTROL_H_
// This logic was borrowed ( then namespaced ) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define CONTROLLER_MANAGER_EXPORT __attribute__( ( dllexport ) )
#define CONTROLLER_MANAGER_IMPORT __attribute__( ( dllimport ) )
#else
#define CONTROLLER_MANAGER_EXPORT __declspec( dllexport )
#define CONTROLLER_MANAGER_IMPORT __declspec( dllimport )
#endif
#ifdef CONTROLLER_MANAGER_BUILDING_DLL
#define CONTROLLER_MANAGER_PUBLIC CONTROLLER_MANAGER_EXPORT
#else
#define CONTROLLER_MANAGER_PUBLIC CONTROLLER_MANAGER_IMPORT
#endif
#define CONTROLLER_MANAGER_PUBLIC_TYPE CONTROLLER_MANAGER_PUBLIC
#define CONTROLLER_MANAGER_LOCAL
#else
#define CONTROLLER_MANAGER_EXPORT __attribute__( ( visibility( "default" ) ) )
#define CONTROLLER_MANAGER_IMPORT
#if __GNUC__ >= 4
#define CONTROLLER_MANAGER_PUBLIC __attribute__( ( visibility( "default" ) ) )
#define CONTROLLER_MANAGER_LOCAL __attribute__( ( visibility( "hidden" ) ) )
#else
#define CONTROLLER_MANAGER_PUBLIC
#define CONTROLLER_MANAGER_LOCAL
#endif
#define CONTROLLER_MANAGER_PUBLIC_TYPE
#endif
#endif // CONTROLLER_MANAGER__VISIBILITY_CONTROL_H_
1 // Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "controller_manager/controller_manager.hpp"
#include <list>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "controller_interface/controller_interface_base.hpp"
#include "controller_manager_msgs/msg/hardware_component_state.hpp"
#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/state.hpp"
namespace // utility
{
32 static constexpr const char * kControllerInterfaceNamespace = "controller_interface";
33 static constexpr const char * kControllerInterfaceClassName =
"controller_interface::ControllerInterface";
35 static constexpr const char * kChainableControllerInterfaceClassName =
"controller_interface::ChainableControllerInterface";
// Changed services history QoS to keep all so we don't lose any client service calls
static const rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = {
RMW_QOS_POLICY_HISTORY_KEEP_ALL,
1, // message queue depth
RMW_QOS_POLICY_RELIABILITY_RELIABLE,
RMW_QOS_POLICY_DURABILITY_VOLATILE,
RMW_QOS_DEADLINE_DEFAULT,
RMW_QOS_LIFESPAN_DEFAULT,
RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,
false};
50 inline bool is_controller_inactive( const controller_interface::ControllerInterfaceBase & controller )
{
return controller.get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE;
}
55 inline bool is_controller_inactive(
const controller_interface::ControllerInterfaceBaseSharedPtr & controller )
{
return is_controller_inactive( *controller );
}
61 inline bool is_controller_active( const controller_interface::ControllerInterfaceBase & controller )
{
return controller.get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE;
}
66 inline bool is_controller_active(
const controller_interface::ControllerInterfaceBaseSharedPtr & controller )
{
return is_controller_active( *controller );
}
72 bool controller_name_compare( const controller_manager::ControllerSpec & a, const std::string & name )
{
return a.info.name == name;
}
/// Checks if a command interface belongs to a controller based on its prefix.
/**
* A command interface can be provided by a controller in which case is called "reference"
* interface.
* This means that the @interface_name starts with the name of a controller.
*
* \param[in] interface_name to be found in the map.
* \param[in] controllers list of controllers to compare their names to interface's prefix.
* \param[out] following_controller_it iterator to the following controller that reference interface
* @interface_name belongs to.
* \return true if interface has a controller name as prefix, false otherwise.
*/
89 bool command_interface_is_reference_interface_of_controller(
const std::string interface_name,
const std::vector<controller_manager::ControllerSpec> & controllers,
controller_manager::ControllersListIterator & following_controller_it )
{
auto split_pos = interface_name.find_first_of( '/' );
if ( split_pos == std::string::npos ) // '/' exist in the string ( should be always false )
{
RCLCPP_FATAL(
rclcpp::get_logger( "ControllerManager::utils" ),
"Character '/', was not find in the interface name '%s'. This should never happen. "
"Stop the controller manager immediately and restart it.",
interface_name.c_str( ) );
throw std::runtime_error( "Mismatched interface name. See the FATAL message above." );
}
auto interface_prefix = interface_name.substr( 0, split_pos );
following_controller_it = std::find_if(
controllers.begin( ), controllers.end( ),
std::bind( controller_name_compare, std::placeholders::_1, interface_prefix ) );
RCLCPP_DEBUG(
rclcpp::get_logger( "ControllerManager::utils" ),
"Deduced interface prefix '%s' - searching for the controller with the same name.",
interface_prefix.c_str( ) );
if ( following_controller_it == controllers.end( ) )
{
RCLCPP_DEBUG(
rclcpp::get_logger( "ControllerManager::utils" ),
"Required command interface '%s' with prefix '%s' is not reference interface.",
interface_name.c_str( ), interface_prefix.c_str( ) );
return false;
}
return true;
}
} // namespace
129 namespace controller_manager
{
rclcpp::NodeOptions get_cm_node_options( )
{
rclcpp::NodeOptions node_options;
// Required for getting types of controllers to be loaded via service call
node_options.allow_undeclared_parameters( true );
node_options.automatically_declare_parameters_from_overrides( true );
return node_options;
}
ControllerManager::ControllerManager(
std::shared_ptr<rclcpp::Executor> executor, const std::string & manager_node_name,
const std::string & namespace_ )
: rclcpp::Node( manager_node_name, namespace_, get_cm_node_options( ) ),
resource_manager_( std::make_unique<hardware_interface::ResourceManager>( ) ),
executor_( executor ),
loader_( std::make_shared<pluginlib::ClassLoader<controller_interface::ControllerInterface>>(
kControllerInterfaceNamespace, kControllerInterfaceClassName ) ),
chainable_loader_(
std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
kControllerInterfaceNamespace, kChainableControllerInterfaceClassName ) )
{
if ( !get_parameter( "update_rate", update_rate_ ) )
{
RCLCPP_WARN( get_logger( ), "'update_rate' parameter not set, using default value." );
}
std::string robot_description = "";
get_parameter( "robot_description", robot_description );
if ( robot_description.empty( ) )
{
throw std::runtime_error( "Unable to initialize resource manager, no robot description found." );
}
init_resource_manager( robot_description );
init_services( );
}
ControllerManager::ControllerManager(
std::unique_ptr<hardware_interface::ResourceManager> resource_manager,
std::shared_ptr<rclcpp::Executor> executor, const std::string & manager_node_name,
const std::string & namespace_ )
: rclcpp::Node( manager_node_name, namespace_, get_cm_node_options( ) ),
resource_manager_( std::move( resource_manager ) ),
executor_( executor ),
loader_( std::make_shared<pluginlib::ClassLoader<controller_interface::ControllerInterface>>(
kControllerInterfaceNamespace, kControllerInterfaceClassName ) ),
chainable_loader_(
std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
kControllerInterfaceNamespace, kChainableControllerInterfaceClassName ) )
{
init_services( );
}
void ControllerManager::init_resource_manager( const std::string & robot_description )
{
// TODO( destogl ): manage this when there is an error - CM should not die because URDF is wrong...
resource_manager_->load_urdf( robot_description );
using lifecycle_msgs::msg::State;
std::vector<std::string> configure_components_on_start = std::vector<std::string>( {} );
get_parameter( "configure_components_on_start", configure_components_on_start );
rclcpp_lifecycle::State inactive_state(
State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE );
for ( const auto & component : configure_components_on_start )
{
resource_manager_->set_component_state( component, inactive_state );
}
std::vector<std::string> activate_components_on_start = std::vector<std::string>( {} );
get_parameter( "activate_components_on_start", activate_components_on_start );
rclcpp_lifecycle::State active_state(
State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE );
for ( const auto & component : activate_components_on_start )
{
resource_manager_->set_component_state( component, active_state );
}
// if both parameter are empty or non-existing preserve behavior where all components are
// activated per default
if ( configure_components_on_start.empty( ) && activate_components_on_start.empty( ) )
{
resource_manager_->activate_all_components( );
}
}
void ControllerManager::init_services( )
{
// TODO( anyone ): Due to issues with the MutliThreadedExecutor, this control loop does not rely on
// the executor ( see issue #260 ).
// deterministic_callback_group_ = create_callback_group(
// rclcpp::CallbackGroupType::MutuallyExclusive );
best_effort_callback_group_ = create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive );
using namespace std::placeholders;
list_controllers_service_ = create_service<controller_manager_msgs::srv::ListControllers>(
"~/list_controllers", std::bind( &ControllerManager::list_controllers_srv_cb, this, _1, _2 ),
rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_ );
list_controller_types_service_ =
create_service<controller_manager_msgs::srv::ListControllerTypes>(
"~/list_controller_types",
std::bind( &ControllerManager::list_controller_types_srv_cb, this, _1, _2 ),
rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_ );
load_controller_service_ = create_service<controller_manager_msgs::srv::LoadController>(
"~/load_controller", std::bind( &ControllerManager::load_controller_service_cb, this, _1, _2 ),
rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_ );
configure_controller_service_ = create_service<controller_manager_msgs::srv::ConfigureController>(
"~/configure_controller",
std::bind( &ControllerManager::configure_controller_service_cb, this, _1, _2 ),
rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_ );
reload_controller_libraries_service_ =
create_service<controller_manager_msgs::srv::ReloadControllerLibraries>(
"~/reload_controller_libraries",
std::bind( &ControllerManager::reload_controller_libraries_service_cb, this, _1, _2 ),
rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_ );
switch_controller_service_ = create_service<controller_manager_msgs::srv::SwitchController>(
"~/switch_controller",
std::bind( &ControllerManager::switch_controller_service_cb, this, _1, _2 ),
rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_ );
unload_controller_service_ = create_service<controller_manager_msgs::srv::UnloadController>(
"~/unload_controller",
std::bind( &ControllerManager::unload_controller_service_cb, this, _1, _2 ),
rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_ );
list_hardware_components_service_ =
create_service<controller_manager_msgs::srv::ListHardwareComponents>(
"~/list_hardware_components",
std::bind( &ControllerManager::list_hardware_components_srv_cb, this, _1, _2 ),
rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_ );
list_hardware_interfaces_service_ =
create_service<controller_manager_msgs::srv::ListHardwareInterfaces>(
"~/list_hardware_interfaces",
std::bind( &ControllerManager::list_hardware_interfaces_srv_cb, this, _1, _2 ),
rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_ );
set_hardware_component_state_service_ =
create_service<controller_manager_msgs::srv::SetHardwareComponentState>(
"~/set_hardware_component_state",
std::bind( &ControllerManager::set_hardware_component_state_srv_cb, this, _1, _2 ),
rmw_qos_profile_services_hist_keep_all, best_effort_callback_group_ );
}
controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_controller(
const std::string & controller_name, const std::string & controller_type )
{
RCLCPP_INFO( get_logger( ), "Loading controller '%s'", controller_name.c_str( ) );
if (
!loader_->isClassAvailable( controller_type ) &&
!chainable_loader_->isClassAvailable( controller_type ) )
{
RCLCPP_ERROR(
get_logger( ), "Loader for controller '%s' ( type '%s' ) not found.", controller_name.c_str( ),
controller_type.c_str( ) );
RCLCPP_INFO( get_logger( ), "Available classes:" );
for ( const auto & available_class : loader_->getDeclaredClasses( ) )
{
RCLCPP_INFO( get_logger( ), " %s", available_class.c_str( ) );
}
for ( const auto & available_class : chainable_loader_->getDeclaredClasses( ) )
{
RCLCPP_INFO( get_logger( ), " %s", available_class.c_str( ) );
}
return nullptr;
}
RCLCPP_DEBUG( get_logger( ), "Loader for controller '%s' found.", controller_name.c_str( ) );
controller_interface::ControllerInterfaceBaseSharedPtr controller;
if ( loader_->isClassAvailable( controller_type ) )
{
controller = loader_->createSharedInstance( controller_type );
}
if ( chainable_loader_->isClassAvailable( controller_type ) )
{
controller = chainable_loader_->createSharedInstance( controller_type );
}
ControllerSpec controller_spec;
controller_spec.c = controller;
controller_spec.info.name = controller_name;
controller_spec.info.type = controller_type;
return add_controller_impl( controller_spec );
}
controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_controller(
const std::string & controller_name )
{
const std::string param_name = controller_name + ".type";
std::string controller_type;
// We cannot declare the parameters for the controllers that will be loaded in the future,
// because they are plugins and we cannot be aware of all of them.
// So when we're told to load a controller by name, we need to declare the parameter if
// we haven't done so, and then read it.
// Check if parameter has been declared
if ( !has_parameter( param_name ) )
{
declare_parameter( param_name, rclcpp::ParameterType::PARAMETER_STRING );
}
if ( !get_parameter( param_name, controller_type ) )
{
RCLCPP_ERROR(
get_logger( ), "The 'type' param was not defined for '%s'.", controller_name.c_str( ) );
return nullptr;
}
return load_controller( controller_name, controller_type );
}
controller_interface::return_type ControllerManager::unload_controller(
const std::string & controller_name )
{
std::lock_guard<std::recursive_mutex> guard( rt_controllers_wrapper_.controllers_lock_ );
std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list( guard );
const std::vector<ControllerSpec> & from = rt_controllers_wrapper_.get_updated_list( guard );
// Transfers the active controllers over, skipping the one to be removed and the active ones.
to = from;
auto found_it = std::find_if(
to.begin( ), to.end( ),
std::bind( controller_name_compare, std::placeholders::_1, controller_name ) );
if ( found_it == to.end( ) )
{
// Fails if we could not remove the controllers
to.clear( );
RCLCPP_ERROR(
get_logger( ),
"Could not unload controller with name '%s' because no controller with this name exists",
controller_name.c_str( ) );
return controller_interface::return_type::ERROR;
}
auto & controller = *found_it;
if ( is_controller_active( *controller.c ) )
{
to.clear( );
RCLCPP_ERROR(
get_logger( ), "Could not unload controller with name '%s' because it is still active",
controller_name.c_str( ) );
return controller_interface::return_type::ERROR;
}
RCLCPP_DEBUG( get_logger( ), "Cleanup controller" );
// TODO( destogl ): remove reference interface if chainable; i.e., add a separate method for
// cleaning-up controllers?
controller.c->get_node( )->cleanup( );
executor_->remove_node( controller.c->get_node( )->get_node_base_interface( ) );
to.erase( found_it );
// Destroys the old controllers list when the realtime thread is finished with it.
RCLCPP_DEBUG( get_logger( ), "Realtime switches over to new controller list" );
rt_controllers_wrapper_.switch_updated_list( guard );
std::vector<ControllerSpec> & new_unused_list = rt_controllers_wrapper_.get_unused_list( guard );
RCLCPP_DEBUG( get_logger( ), "Destruct controller" );
new_unused_list.clear( );
RCLCPP_DEBUG( get_logger( ), "Destruct controller finished" );
RCLCPP_DEBUG( get_logger( ), "Successfully unloaded controller '%s'", controller_name.c_str( ) );
return controller_interface::return_type::OK;
}
std::vector<ControllerSpec> ControllerManager::get_loaded_controllers( ) const
{
std::lock_guard<std::recursive_mutex> guard( rt_controllers_wrapper_.controllers_lock_ );
return rt_controllers_wrapper_.get_updated_list( guard );
}
controller_interface::return_type ControllerManager::configure_controller(
const std::string & controller_name )
{
RCLCPP_INFO( get_logger( ), "Configuring controller '%s'", controller_name.c_str( ) );
const auto & controllers = get_loaded_controllers( );
auto found_it = std::find_if(
controllers.begin( ), controllers.end( ),
std::bind( controller_name_compare, std::placeholders::_1, controller_name ) );
if ( found_it == controllers.end( ) )
{
RCLCPP_ERROR(
get_logger( ),
"Could not configure controller with name '%s' because no controller with this name exists",
controller_name.c_str( ) );
return controller_interface::return_type::ERROR;
}
auto controller = found_it->c;
auto state = controller->get_state( );
if (
state.id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE ||
state.id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED )
{
RCLCPP_ERROR(
get_logger( ), "Controller '%s' can not be configured from '%s' state.",
controller_name.c_str( ), state.label( ).c_str( ) );
return controller_interface::return_type::ERROR;
}
auto new_state = controller->get_state( );
if ( state.id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE )
{
RCLCPP_DEBUG(
get_logger( ), "Controller '%s' is cleaned-up before configuring", controller_name.c_str( ) );
// TODO( destogl ): remove reference interface if chainable; i.e., add a separate method for
// cleaning-up controllers?
new_state = controller->get_node( )->cleanup( );
if ( new_state.id( ) != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED )
{
RCLCPP_ERROR(
get_logger( ), "Controller '%s' can not be cleaned-up before configuring",
controller_name.c_str( ) );
return controller_interface::return_type::ERROR;
}
}
new_state = controller->configure( );
if ( new_state.id( ) != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE )
{
RCLCPP_ERROR(
get_logger( ), "After configuring, controller '%s' is in state '%s' , expected inactive.",
controller_name.c_str( ), new_state.label( ).c_str( ) );
return controller_interface::return_type::ERROR;
}
// CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers
if ( controller->is_chainable( ) )
{
RCLCPP_DEBUG(
get_logger( ),
"Controller '%s' is chainable. Interfaces are being exported to resource manager.",
controller_name.c_str( ) );
auto interfaces = controller->export_reference_interfaces( );
if ( interfaces.empty( ) )
{
// TODO( destogl ): Add test for this!
RCLCPP_ERROR(
get_logger( ), "Controller '%s' is chainable, but does not export any reference interfaces.",
controller_name.c_str( ) );
return controller_interface::return_type::ERROR;
}
resource_manager_->import_controller_reference_interfaces( controller_name, interfaces );
// TODO( destogl ): check and resort controllers in the vector
}
return controller_interface::return_type::OK;
}
void ControllerManager::clear_requests( )
{
deactivate_request_.clear( );
activate_request_.clear( );
to_chained_mode_request_.clear( );
from_chained_mode_request_.clear( );
activate_command_interface_request_.clear( );
deactivate_command_interface_request_.clear( );
}
controller_interface::return_type ControllerManager::switch_controller(
const std::vector<std::string> & activate_controllers,
const std::vector<std::string> & deactivate_controllers, int strictness, bool activate_asap,
const rclcpp::Duration & timeout )
{
switch_params_ = SwitchParams( );
if ( !deactivate_request_.empty( ) || !activate_request_.empty( ) )
{
RCLCPP_FATAL(
get_logger( ),
"The internal deactivate and activate request lists are not empty at the beginning of the "
"switch_controller( ) call. This should never happen." );
throw std::runtime_error( "CM's internal state is not correct. See the FATAL message above." );
}
if (
!deactivate_command_interface_request_.empty( ) || !activate_command_interface_request_.empty( ) )
{
RCLCPP_FATAL(
get_logger( ),
"The internal deactivate and activat requests command interface lists are not empty at the "
"switch_controller( ) call. This should never happen." );
throw std::runtime_error( "CM's internal state is not correct. See the FATAL message above." );
}
if ( !from_chained_mode_request_.empty( ) || !to_chained_mode_request_.empty( ) )
{
RCLCPP_FATAL(
get_logger( ),
"The internal 'from' and 'to' chained mode requests are not empty at the "
"switch_controller( ) call. This should never happen." );
throw std::runtime_error( "CM's internal state is not correct. See the FATAL message above." );
}
if ( strictness == 0 )
{
RCLCPP_WARN(
get_logger( ),
"Controller Manager: to switch controllers you need to specify a "
"strictness level of controller_manager_msgs::SwitchController::STRICT "
"( %d ) or ::BEST_EFFORT ( %d ). Defaulting to ::BEST_EFFORT",
controller_manager_msgs::srv::SwitchController::Request::STRICT,
controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT );
strictness = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;
}
RCLCPP_DEBUG( get_logger( ), "Switching controllers:" );
for ( const auto & controller : activate_controllers )
{
RCLCPP_DEBUG( get_logger( ), "- Activating controller '%s'", controller.c_str( ) );
}
for ( const auto & controller : deactivate_controllers )
{
RCLCPP_DEBUG( get_logger( ), "- Deactivating controller '%s'", controller.c_str( ) );
}
const auto list_controllers = [this, strictness](
const std::vector<std::string> & controller_list,
std::vector<std::string> & request_list,
const std::string & action )
{
// lock controllers
std::lock_guard<std::recursive_mutex> guard( rt_controllers_wrapper_.controllers_lock_ );
// list all controllers to ( de )activate
for ( const auto & controller : controller_list )
{
const auto & updated_controllers = rt_controllers_wrapper_.get_updated_list( guard );
auto found_it = std::find_if(
updated_controllers.begin( ), updated_controllers.end( ),
std::bind( controller_name_compare, std::placeholders::_1, controller ) );
if ( found_it == updated_controllers.end( ) )
{
RCLCPP_WARN(
get_logger( ),
"Could not '%s' controller with name '%s' because no controller with this name exists",
action.c_str( ), controller.c_str( ) );
if ( strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT )
{
RCLCPP_ERROR( get_logger( ), "Aborting, no controller is switched! ( 'STRICT' switch )" );
return controller_interface::return_type::ERROR;
}
}
else
{
RCLCPP_DEBUG(
get_logger( ), "Found controller '%s' that needs to be %sed in list of controllers",
controller.c_str( ), action.c_str( ) );
request_list.push_back( controller );
}
}
RCLCPP_DEBUG(
get_logger( ), "'%s' request vector has size %i", action.c_str( ), ( int )request_list.size( ) );
return controller_interface::return_type::OK;
};
// list all controllers to deactivate ( check if all controllers exist )
auto ret = list_controllers( deactivate_controllers, deactivate_request_, "deactivate" );
if ( ret != controller_interface::return_type::OK )
{
deactivate_request_.clear( );
return ret;
}
// list all controllers to activate ( check if all controllers exist )
ret = list_controllers( activate_controllers, activate_request_, "activate" );
if ( ret != controller_interface::return_type::OK )
{
deactivate_request_.clear( );
activate_request_.clear( );
return ret;
}
// lock controllers
std::lock_guard<std::recursive_mutex> guard( rt_controllers_wrapper_.controllers_lock_ );
const std::vector<ControllerSpec> & controllers = rt_controllers_wrapper_.get_updated_list( guard );
// if a preceding controller is deactivated, all first-level controllers should be switched 'from'
// chained mode
propagate_deactivation_of_chained_mode( controllers );
// check if controllers should be switched 'to' chained mode when controllers are activated
for ( auto ctrl_it = activate_request_.begin( ); ctrl_it != activate_request_.end( ); ++ctrl_it )
{
auto controller_it = std::find_if(
controllers.begin( ), controllers.end( ),
std::bind( controller_name_compare, std::placeholders::_1, *ctrl_it ) );
controller_interface::return_type ret = controller_interface::return_type::OK;
// if controller is not inactive then do not do any following-controllers checks
if ( !is_controller_inactive( controller_it->c ) )
{
RCLCPP_WARN(
get_logger( ),
"Controller with name '%s' is not inactive so its following"
"controllers do not have to be checked, because it cannot be activated.",
controller_it->info.name.c_str( ) );
ret = controller_interface::return_type::ERROR;
}
else
{
ret = check_following_controllers_for_activate( controllers, strictness, controller_it );
}
if ( ret != controller_interface::return_type::OK )
{
RCLCPP_WARN(
get_logger( ),
"Could not activate controller with name '%s'. Check above warnings for more details. "
"Check the state of the controllers and their required interfaces using "
"`ros2 control list_controllers -v` CLI to get more information.",
( *ctrl_it ).c_str( ) );
if ( strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT )
{
// TODO( destogl ): automatic manipulation of the chain:
// || strictness ==
// controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN );
// remove controller that can not be activated from the activation request and step-back
// iterator to correctly step to the next element in the list in the loop
activate_request_.erase( ctrl_it );
--ctrl_it;
}
if ( strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT )
{
RCLCPP_ERROR( get_logger( ), "Aborting, no controller is switched! ( ::STRICT switch )" );
// reset all lists
clear_requests( );
return controller_interface::return_type::ERROR;
}
}
}
// check if controllers should be deactivated if used in chained mode
for ( auto ctrl_it = deactivate_request_.begin( ); ctrl_it != deactivate_request_.end( ); ++ctrl_it )
{
auto controller_it = std::find_if(
controllers.begin( ), controllers.end( ),
std::bind( controller_name_compare, std::placeholders::_1, *ctrl_it ) );
controller_interface::return_type ret = controller_interface::return_type::OK;
// if controller is not active then skip preceding-controllers checks
if ( !is_controller_active( controller_it->c ) )
{
RCLCPP_WARN(
get_logger( ), "Controller with name '%s' can not be deactivated since it is not active.",
controller_it->info.name.c_str( ) );
ret = controller_interface::return_type::ERROR;
}
else
{
ret = check_preceeding_controllers_for_deactivate( controllers, strictness, controller_it );
}
if ( ret != controller_interface::return_type::OK )
{
RCLCPP_WARN(
get_logger( ),
"Could not deactivate controller with name '%s'. Check above warnings for more details. "
"Check the state of the controllers and their required interfaces using "
"`ros2 control list_controllers -v` CLI to get more information.",
( *ctrl_it ).c_str( ) );
if ( strictness == controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT )
{
// remove controller that can not be activated from the activation request and step-back
// iterator to correctly step to the next element in the list in the loop
deactivate_request_.erase( ctrl_it );
--ctrl_it;
}
if ( strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT )
{
RCLCPP_ERROR( get_logger( ), "Aborting, no controller is switched! ( ::STRICT switch )" );
// reset all lists
clear_requests( );
return controller_interface::return_type::ERROR;
}
}
}
for ( const auto & controller : controllers )
{
auto to_chained_mode_list_it = std::find(
to_chained_mode_request_.begin( ), to_chained_mode_request_.end( ), controller.info.name );
bool in_to_chained_mode_list = to_chained_mode_list_it != to_chained_mode_request_.end( );
auto from_chained_mode_list_it = std::find(
from_chained_mode_request_.begin( ), from_chained_mode_request_.end( ), controller.info.name );
bool in_from_chained_mode_list = from_chained_mode_list_it != from_chained_mode_request_.end( );
auto deactivate_list_it =
std::find( deactivate_request_.begin( ), deactivate_request_.end( ), controller.info.name );
bool in_deactivate_list = deactivate_list_it != deactivate_request_.end( );
const bool is_active = is_controller_active( *controller.c );
const bool is_inactive = is_controller_inactive( *controller.c );
// restart controllers that need to switch their 'chained mode' - add to ( de )activate lists
if ( in_to_chained_mode_list || in_from_chained_mode_list )
{
if ( is_active && !in_deactivate_list )
{
deactivate_request_.push_back( controller.info.name );
activate_request_.push_back( controller.info.name );
}
}
// get pointers to places in deactivate and activate lists ( ( de )activate lists have changed )
deactivate_list_it =
std::find( deactivate_request_.begin( ), deactivate_request_.end( ), controller.info.name );
in_deactivate_list = deactivate_list_it != deactivate_request_.end( );
auto activate_list_it =
std::find( activate_request_.begin( ), activate_request_.end( ), controller.info.name );
bool in_activate_list = activate_list_it != activate_request_.end( );
auto handle_conflict = [&]( const std::string & msg )
{
if ( strictness == controller_manager_msgs::srv::SwitchController::Request::STRICT )
{
RCLCPP_ERROR( get_logger( ), "%s", msg.c_str( ) );
deactivate_request_.clear( );
deactivate_command_interface_request_.clear( );
activate_request_.clear( );
activate_command_interface_request_.clear( );
to_chained_mode_request_.clear( );
from_chained_mode_request_.clear( );
return controller_interface::return_type::ERROR;
}
RCLCPP_WARN( get_logger( ), "%s", msg.c_str( ) );
return controller_interface::return_type::OK;
};
// check for double stop
if ( !is_active && in_deactivate_list )
{
auto ret = handle_conflict(
"Could not deactivate controller '" + controller.info.name + "' since it is not active" );
if ( ret != controller_interface::return_type::OK )
{
return ret;
}
in_deactivate_list = false;
deactivate_request_.erase( deactivate_list_it );
}
// check for doubled activation
if ( is_active && !in_deactivate_list && in_activate_list )
{
auto ret = handle_conflict(
"Could not activate controller '" + controller.info.name + "' since it is already active" );
if ( ret != controller_interface::return_type::OK )
{
return ret;
}
in_activate_list = false;
activate_request_.erase( activate_list_it );
}
// check for illegal activation of an unconfigured/finalized controller
if ( !is_inactive && !in_deactivate_list && in_activate_list )
{
auto ret = handle_conflict(
"Could not activate controller '" + controller.info.name +
"' since it is not in inactive state" );
if ( ret != controller_interface::return_type::OK )
{
return ret;
}
in_activate_list = false;
activate_request_.erase( activate_list_it );
}
const auto extract_interfaces_for_controller =
[this]( const ControllerSpec controller, std::vector<std::string> & request_interface_list )
{
auto command_interface_config = controller.c->command_interface_configuration( );
std::vector<std::string> command_interface_names = {};
if ( command_interface_config.type == controller_interface::interface_configuration_type::ALL )
{
command_interface_names = resource_manager_->available_command_interfaces( );
}
if (
command_interface_config.type ==
controller_interface::interface_configuration_type::INDIVIDUAL )
{
command_interface_names = command_interface_config.names;
}
request_interface_list.insert(
request_interface_list.end( ), command_interface_names.begin( ),
command_interface_names.end( ) );
};
if ( in_activate_list )
{
extract_interfaces_for_controller( controller, activate_command_interface_request_ );
}
if ( in_deactivate_list )
{
extract_interfaces_for_controller( controller, deactivate_command_interface_request_ );
}
}
if ( activate_request_.empty( ) && deactivate_request_.empty( ) )
{
RCLCPP_INFO( get_logger( ), "Empty activate and deactivate list, not requesting switch" );
clear_requests( );
return controller_interface::return_type::OK;
}
if (
!activate_command_interface_request_.empty( ) || !deactivate_command_interface_request_.empty( ) )
{
if ( !resource_manager_->prepare_command_mode_switch(
activate_command_interface_request_, deactivate_command_interface_request_ ) )
{
RCLCPP_ERROR(
get_logger( ),
"Could not switch controllers since prepare command mode switch was rejected." );
clear_requests( );
return controller_interface::return_type::ERROR;
}
}
// start the atomic controller switching
switch_params_.strictness = strictness;
switch_params_.activate_asap = activate_asap;
switch_params_.init_time = rclcpp::Clock( ).now( );
switch_params_.timeout = timeout;
switch_params_.do_switch = true;
// wait until switch is finished
RCLCPP_DEBUG( get_logger( ), "Requested atomic controller switch from realtime loop" );
while ( rclcpp::ok( ) && switch_params_.do_switch )
{
if ( !rclcpp::ok( ) )
{
return controller_interface::return_type::ERROR;
}
std::this_thread::sleep_for( std::chrono::microseconds( 100 ) );
}
// copy the controllers spec from the used to the unused list
std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list( guard );
to = controllers;
// update the claimed interface controller info
for ( auto & controller : to )
{
if ( is_controller_active( controller.c ) )
{
auto command_interface_config = controller.c->command_interface_configuration( );
if ( command_interface_config.type == controller_interface::interface_configuration_type::ALL )
{
controller.info.claimed_interfaces = resource_manager_->available_command_interfaces( );
}
if (
command_interface_config.type ==
controller_interface::interface_configuration_type::INDIVIDUAL )
{
controller.info.claimed_interfaces = command_interface_config.names;
}
}
else
{
controller.info.claimed_interfaces.clear( );
}
}
// switch lists
rt_controllers_wrapper_.switch_updated_list( guard );
// clear unused list
rt_controllers_wrapper_.get_unused_list( guard ).clear( );
clear_requests( );
RCLCPP_DEBUG( get_logger( ), "Successfully switched controllers" );
return controller_interface::return_type::OK;
}
controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_controller_impl(
const ControllerSpec & controller )
{
// lock controllers
std::lock_guard<std::recursive_mutex> guard( rt_controllers_wrapper_.controllers_lock_ );
std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list( guard );
const std::vector<ControllerSpec> & from = rt_controllers_wrapper_.get_updated_list( guard );
// Copy all controllers from the 'from' list to the 'to' list
to = from;
auto found_it = std::find_if(
to.begin( ), to.end( ),
std::bind( controller_name_compare, std::placeholders::_1, controller.info.name ) );
// Checks that we're not duplicating controllers
if ( found_it != to.end( ) )
{
to.clear( );
RCLCPP_ERROR(
get_logger( ), "A controller named '%s' was already loaded inside the controller manager",
controller.info.name.c_str( ) );
return nullptr;
}
if (
controller.c->init( controller.info.name, get_namespace( ) ) ==
controller_interface::return_type::ERROR )
{
to.clear( );
RCLCPP_ERROR(
get_logger( ), "Could not initialize the controller named '%s'", controller.info.name.c_str( ) );
return nullptr;
}
// ensure controller's `use_sim_time` parameter matches controller_manager's
const rclcpp::Parameter use_sim_time = this->get_parameter( "use_sim_time" );
if ( use_sim_time.as_bool( ) )
{
RCLCPP_INFO(
get_logger( ),
"Setting use_sim_time=True for %s to match controller manager "
"( see ros2_control#325 for details )",
controller.info.name.c_str( ) );
controller.c->get_node( )->set_parameter( use_sim_time );
}
executor_->add_node( controller.c->get_node( )->get_node_base_interface( ) );
to.emplace_back( controller );
// Destroys the old controllers list when the realtime thread is finished with it.
RCLCPP_DEBUG( get_logger( ), "Realtime switches over to new controller list" );
rt_controllers_wrapper_.switch_updated_list( guard );
RCLCPP_DEBUG( get_logger( ), "Destruct controller" );
std::vector<ControllerSpec> & new_unused_list = rt_controllers_wrapper_.get_unused_list( guard );
new_unused_list.clear( );
RCLCPP_DEBUG( get_logger( ), "Destruct controller finished" );
return to.back( ).c;
}
void ControllerManager::manage_switch( )
{
// Ask hardware interfaces to change mode
if ( !resource_manager_->perform_command_mode_switch(
activate_command_interface_request_, deactivate_command_interface_request_ ) )
{
RCLCPP_ERROR( get_logger( ), "Error while performing mode switch." );
}
deactivate_controllers( );
switch_chained_mode( to_chained_mode_request_, true );
switch_chained_mode( from_chained_mode_request_, false );
// activate controllers once the switch is fully complete
if ( !switch_params_.activate_asap )
{
activate_controllers( );
}
else
{
// activate controllers as soon as their required joints are done switching
activate_controllers_asap( );
}
// TODO( destogl ): move here "do_switch = false"
}
void ControllerManager::deactivate_controllers( )
{
std::vector<ControllerSpec> & rt_controller_list =
rt_controllers_wrapper_.update_and_get_used_by_rt_list( );
// stop controllers
for ( const auto & request : deactivate_request_ )
{
auto found_it = std::find_if(
rt_controller_list.begin( ), rt_controller_list.end( ),
std::bind( controller_name_compare, std::placeholders::_1, request ) );
if ( found_it == rt_controller_list.end( ) )
{
RCLCPP_ERROR(
get_logger( ),
"Got request to stop controller '%s' but it is not in the realtime controller list",
request.c_str( ) );
continue;
}
auto controller = found_it->c;
if ( is_controller_active( *controller ) )
{
const auto new_state = controller->get_node( )->deactivate( );
controller->release_interfaces( );
if ( new_state.id( ) != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE )
{
RCLCPP_ERROR(
get_logger( ), "After deactivating, controller '%s' is in state '%s', expected Inactive",
request.c_str( ), new_state.label( ).c_str( ) );
}
}
}
}
void ControllerManager::switch_chained_mode(
const std::vector<std::string> & chained_mode_switch_list, bool to_chained_mode )
{
std::vector<ControllerSpec> & rt_controller_list =
rt_controllers_wrapper_.update_and_get_used_by_rt_list( );
for ( const auto & request : chained_mode_switch_list )
{
auto found_it = std::find_if(
rt_controller_list.begin( ), rt_controller_list.end( ),
std::bind( controller_name_compare, std::placeholders::_1, request ) );
if ( found_it == rt_controller_list.end( ) )
{
RCLCPP_FATAL(
get_logger( ),
"Got request to turn %s chained mode for controller '%s', but controller is not in the "
"realtime controller list. ( This should never happen! )",
( to_chained_mode ? "ON" : "OFF" ), request.c_str( ) );
continue;
}
auto controller = found_it->c;
if ( !is_controller_active( *controller ) )
{
if ( controller->set_chained_mode( to_chained_mode ) )
{
if ( to_chained_mode )
{
resource_manager_->make_controller_reference_interfaces_available( request );
}
else
{
resource_manager_->make_controller_reference_interfaces_unavailable( request );
}
}
else
{
RCLCPP_ERROR(
get_logger( ),
"Got request to turn %s chained mode for controller '%s', but controller refused to do "
"it! The control will probably not work as expected. Try to restart all controllers. "
"If "
"the error persist check controllers' individual configuration.",
( to_chained_mode ? "ON" : "OFF" ), request.c_str( ) );
}
}
else
{
RCLCPP_FATAL(
get_logger( ),
"Got request to turn %s chained mode for controller '%s', but this can not happen if "
"controller is in '%s' state. ( This should never happen! )",
( to_chained_mode ? "ON" : "OFF" ), request.c_str( ),
hardware_interface::lifecycle_state_names::ACTIVE );
}
}
}
void ControllerManager::activate_controllers( )
{
std::vector<ControllerSpec> & rt_controller_list =
rt_controllers_wrapper_.update_and_get_used_by_rt_list( );
for ( const auto & request : activate_request_ )
{
auto found_it = std::find_if(
rt_controller_list.begin( ), rt_controller_list.end( ),
std::bind( controller_name_compare, std::placeholders::_1, request ) );
if ( found_it == rt_controller_list.end( ) )
{
RCLCPP_ERROR(
get_logger( ),
"Got request to activate controller '%s' but it is not in the realtime controller list",
request.c_str( ) );
continue;
}
auto controller = found_it->c;
bool assignment_successful = true;
// assign command interfaces to the controller
auto command_interface_config = controller->command_interface_configuration( );
// default to controller_interface::configuration_type::NONE
std::vector<std::string> command_interface_names = {};
if ( command_interface_config.type == controller_interface::interface_configuration_type::ALL )
{
command_interface_names = resource_manager_->available_command_interfaces( );
}
if (
command_interface_config.type ==
controller_interface::interface_configuration_type::INDIVIDUAL )
{
command_interface_names = command_interface_config.names;
}
std::vector<hardware_interface::LoanedCommandInterface> command_loans;
command_loans.reserve( command_interface_names.size( ) );
for ( const auto & command_interface : command_interface_names )
{
if ( resource_manager_->command_interface_is_claimed( command_interface ) )
{
RCLCPP_ERROR(
get_logger( ),
"Resource conflict for controller '%s'. Command interface '%s' is already claimed.",
request.c_str( ), command_interface.c_str( ) );
assignment_successful = false;
break;
}
try
{
command_loans.emplace_back( resource_manager_->claim_command_interface( command_interface ) );
}
catch ( const std::exception & e )
{
RCLCPP_ERROR( get_logger( ), "Can't activate controller '%s': %s", request.c_str( ), e.what( ) );
assignment_successful = false;
break;
}
}
// something went wrong during command interfaces, go skip the controller
if ( !assignment_successful )
{
continue;
}
// assign state interfaces to the controller
auto state_interface_config = controller->state_interface_configuration( );
// default to controller_interface::configuration_type::NONE
std::vector<std::string> state_interface_names = {};
if ( state_interface_config.type == controller_interface::interface_configuration_type::ALL )
{
state_interface_names = resource_manager_->available_state_interfaces( );
}
if (
state_interface_config.type == controller_interface::interface_configuration_type::INDIVIDUAL )
{
state_interface_names = state_interface_config.names;
}
std::vector<hardware_interface::LoanedStateInterface> state_loans;
state_loans.reserve( state_interface_names.size( ) );
for ( const auto & state_interface : state_interface_names )
{
try
{
state_loans.emplace_back( resource_manager_->claim_state_interface( state_interface ) );
}
catch ( const std::exception & e )
{
RCLCPP_ERROR( get_logger( ), "Can't activate controller '%s': %s", request.c_str( ), e.what( ) );
assignment_successful = false;
break;
}
}
// something went wrong during state interfaces, go skip the controller
if ( !assignment_successful )
{
continue;
}
controller->assign_interfaces( std::move( command_loans ), std::move( state_loans ) );
const auto new_state = controller->get_node( )->activate( );
if ( new_state.id( ) != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
{
RCLCPP_ERROR(
get_logger( ), "After activating, controller '%s' is in state '%s', expected Active",
controller->get_node( )->get_name( ), new_state.label( ).c_str( ) );
}
}
// All controllers activated, switching done
switch_params_.do_switch = false;
}
void ControllerManager::activate_controllers_asap( )
{
// https://github.com/ros-controls/ros2_control/issues/263
activate_controllers( );
}
void ControllerManager::list_controllers_srv_cb(
const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request>,
std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response> response )
{
// lock services
RCLCPP_DEBUG( get_logger( ), "list controller service called" );
std::lock_guard<std::mutex> services_guard( services_lock_ );
RCLCPP_DEBUG( get_logger( ), "list controller service locked" );
// lock controllers
std::lock_guard<std::recursive_mutex> guard( rt_controllers_wrapper_.controllers_lock_ );
const std::vector<ControllerSpec> & controllers = rt_controllers_wrapper_.get_updated_list( guard );
response->controller.resize( controllers.size( ) );
for ( size_t i = 0; i < controllers.size( ); ++i )
{
controller_manager_msgs::msg::ControllerState & cs = response->controller[i];
cs.name = controllers[i].info.name;
cs.type = controllers[i].info.type;
cs.claimed_interfaces = controllers[i].info.claimed_interfaces;
cs.state = controllers[i].c->get_state( ).label( );
// Get information about interfaces if controller are in 'inactive' or 'active' state
if ( is_controller_active( controllers[i].c ) || is_controller_inactive( controllers[i].c ) )
{
auto command_interface_config = controllers[i].c->command_interface_configuration( );
if ( command_interface_config.type == controller_interface::interface_configuration_type::ALL )
{
cs.required_command_interfaces = resource_manager_->command_interface_keys( );
}
else if (
command_interface_config.type ==
controller_interface::interface_configuration_type::INDIVIDUAL )
{
cs.required_command_interfaces = command_interface_config.names;
}
auto state_interface_config = controllers[i].c->state_interface_configuration( );
if ( state_interface_config.type == controller_interface::interface_configuration_type::ALL )
{
cs.required_state_interfaces = resource_manager_->state_interface_keys( );
}
else if (
state_interface_config.type ==
controller_interface::interface_configuration_type::INDIVIDUAL )
{
cs.required_state_interfaces = state_interface_config.names;
}
}
}
RCLCPP_DEBUG( get_logger( ), "list controller service finished" );
}
void ControllerManager::list_controller_types_srv_cb(
const std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Request>,
std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Response> response )
{
// lock services
RCLCPP_DEBUG( get_logger( ), "list types service called" );
std::lock_guard<std::mutex> guard( services_lock_ );
RCLCPP_DEBUG( get_logger( ), "list types service locked" );
auto cur_types = loader_->getDeclaredClasses( );
for ( const auto & cur_type : cur_types )
{
response->types.push_back( cur_type );
response->base_classes.push_back( kControllerInterfaceClassName );
RCLCPP_DEBUG( get_logger( ), "%s", cur_type.c_str( ) );
}
cur_types = chainable_loader_->getDeclaredClasses( );
for ( const auto & cur_type : cur_types )
{
response->types.push_back( cur_type );
response->base_classes.push_back( kChainableControllerInterfaceClassName );
RCLCPP_DEBUG( get_logger( ), "%s", cur_type.c_str( ) );
}
RCLCPP_DEBUG( get_logger( ), "list types service finished" );
}
void ControllerManager::load_controller_service_cb(
const std::shared_ptr<controller_manager_msgs::srv::LoadController::Request> request,
std::shared_ptr<controller_manager_msgs::srv::LoadController::Response> response )
{
// lock services
RCLCPP_DEBUG( get_logger( ), "loading service called for controller '%s' ", request->name.c_str( ) );
std::lock_guard<std::mutex> guard( services_lock_ );
RCLCPP_DEBUG( get_logger( ), "loading service locked" );
response->ok = load_controller( request->name ).get( ) != nullptr;
RCLCPP_DEBUG(
get_logger( ), "loading service finished for controller '%s' ", request->name.c_str( ) );
}
void ControllerManager::configure_controller_service_cb(
const std::shared_ptr<controller_manager_msgs::srv::ConfigureController::Request> request,
std::shared_ptr<controller_manager_msgs::srv::ConfigureController::Response> response )
{
// lock services
RCLCPP_DEBUG(
get_logger( ), "configuring service called for controller '%s' ", request->name.c_str( ) );
std::lock_guard<std::mutex> guard( services_lock_ );
RCLCPP_DEBUG( get_logger( ), "configuring service locked" );
response->ok = configure_controller( request->name ) == controller_interface::return_type::OK;
RCLCPP_DEBUG(
get_logger( ), "configuring service finished for controller '%s' ", request->name.c_str( ) );
}
void ControllerManager::reload_controller_libraries_service_cb(
const std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Request> request,
std::shared_ptr<controller_manager_msgs::srv::ReloadControllerLibraries::Response> response )
{
// lock services
RCLCPP_DEBUG( get_logger( ), "reload libraries service called" );
std::lock_guard<std::mutex> guard( services_lock_ );
RCLCPP_DEBUG( get_logger( ), "reload libraries service locked" );
// only reload libraries if no controllers are active
std::vector<std::string> loaded_controllers, active_controllers;
loaded_controllers = get_controller_names( );
{
// lock controllers
std::lock_guard<std::recursive_mutex> guard( rt_controllers_wrapper_.controllers_lock_ );
for ( const auto & controller : rt_controllers_wrapper_.get_updated_list( guard ) )
{
if ( is_controller_active( *controller.c ) )
{
active_controllers.push_back( controller.info.name );
}
}
}
if ( !active_controllers.empty( ) && !request->force_kill )
{
RCLCPP_ERROR(
get_logger( ),
"Controller manager: Cannot reload controller libraries because"
" there are still %i active controllers",
( int )active_controllers.size( ) );
response->ok = false;
return;
}
// stop active controllers if requested
if ( !loaded_controllers.empty( ) )
{
RCLCPP_INFO( get_logger( ), "Controller manager: Stopping all active controllers" );
std::vector<std::string> empty;
if (
switch_controller(
empty, active_controllers,
controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT ) !=
controller_interface::return_type::OK )
{
RCLCPP_ERROR(
get_logger( ),
"Controller manager: Cannot reload controller libraries because failed to stop "
"active controllers" );
response->ok = false;
return;
}
for ( const auto & controller : loaded_controllers )
{
if ( unload_controller( controller ) != controller_interface::return_type::OK )
{
RCLCPP_ERROR(
get_logger( ),
"Controller manager: Cannot reload controller libraries because "
"failed to unload controller '%s'",
controller.c_str( ) );
response->ok = false;
return;
}
}
loaded_controllers = get_controller_names( );
}
assert( loaded_controllers.empty( ) );
// Force a reload on all the PluginLoaders ( internally, this recreates the plugin loaders )
loader_ = std::make_shared<pluginlib::ClassLoader<controller_interface::ControllerInterface>>(
kControllerInterfaceNamespace, kControllerInterfaceClassName );
chainable_loader_ =
std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
kControllerInterfaceNamespace, kChainableControllerInterfaceClassName );
RCLCPP_INFO(
get_logger( ), "Controller manager: reloaded controller libraries for '%s'",
kControllerInterfaceNamespace );
response->ok = true;
RCLCPP_DEBUG( get_logger( ), "reload libraries service finished" );
}
void ControllerManager::switch_controller_service_cb(
const std::shared_ptr<controller_manager_msgs::srv::SwitchController::Request> request,
std::shared_ptr<controller_manager_msgs::srv::SwitchController::Response> response )
{
// lock services
RCLCPP_DEBUG( get_logger( ), "switching service called" );
std::lock_guard<std::mutex> guard( services_lock_ );
RCLCPP_DEBUG( get_logger( ), "switching service locked" );
// response->ok = switch_controller(
// request->activate_controllers, request->deactivate_controllers, request->strictness,
// request->activate_asap, request->timeout ) == controller_interface::return_type::OK;
// TODO( destogl ): remove this after deprecated fields are removed from service and use the
// commented three lines above
// BEGIN: remove when deprecated removed
auto activate_controllers = request->activate_controllers;
auto deactivate_controllers = request->deactivate_controllers;
if ( !request->start_controllers.empty( ) )
{
RCLCPP_WARN(
get_logger( ),
"'start_controllers' field is deprecated, use 'activate_controllers' field instead!" );
activate_controllers.insert(
activate_controllers.end( ), request->start_controllers.begin( ),
request->start_controllers.end( ) );
}
if ( !request->stop_controllers.empty( ) )
{
RCLCPP_WARN(
get_logger( ),
"'stop_controllers' field is deprecated, use 'deactivate_controllers' field instead!" );
deactivate_controllers.insert(
deactivate_controllers.end( ), request->stop_controllers.begin( ),
request->stop_controllers.end( ) );
}
auto activate_asap = request->activate_asap;
if ( request->start_asap )
{
RCLCPP_WARN(
get_logger( ), "'start_asap' field is deprecated, use 'activate_asap' field instead!" );
activate_asap = request->start_asap;
}
response->ok = switch_controller(
activate_controllers, deactivate_controllers, request->strictness, activate_asap,
request->timeout ) == controller_interface::return_type::OK;
// END: remove when deprecated removed
RCLCPP_DEBUG( get_logger( ), "switching service finished" );
}
void ControllerManager::unload_controller_service_cb(
const std::shared_ptr<controller_manager_msgs::srv::UnloadController::Request> request,
std::shared_ptr<controller_manager_msgs::srv::UnloadController::Response> response )
{
// lock services
RCLCPP_DEBUG(
get_logger( ), "unloading service called for controller '%s' ", request->name.c_str( ) );
std::lock_guard<std::mutex> guard( services_lock_ );
RCLCPP_DEBUG( get_logger( ), "unloading service locked" );
response->ok = unload_controller( request->name ) == controller_interface::return_type::OK;
RCLCPP_DEBUG(
get_logger( ), "unloading service finished for controller '%s' ", request->name.c_str( ) );
}
void ControllerManager::list_hardware_components_srv_cb(
const std::shared_ptr<controller_manager_msgs::srv::ListHardwareComponents::Request>,
std::shared_ptr<controller_manager_msgs::srv::ListHardwareComponents::Response> response )
{
RCLCPP_DEBUG( get_logger( ), "list hardware components service called" );
std::lock_guard<std::mutex> guard( services_lock_ );
RCLCPP_DEBUG( get_logger( ), "list hardware components service locked" );
auto hw_components_info = resource_manager_->get_components_status( );
response->component.reserve( hw_components_info.size( ) );
for ( const auto & [component_name, component_info] : hw_components_info )
{
auto component = controller_manager_msgs::msg::HardwareComponentState( );
component.name = component_name;
component.type = component_info.type;
component.class_type = component_info.class_type;
component.state.id = component_info.state.id( );
component.state.label = component_info.state.label( );
component.command_interfaces.reserve( component_info.command_interfaces.size( ) );
for ( const auto & interface : component_info.command_interfaces )
{
controller_manager_msgs::msg::HardwareInterface hwi;
hwi.name = interface;
hwi.is_available = resource_manager_->command_interface_is_available( interface );
hwi.is_claimed = resource_manager_->command_interface_is_claimed( interface );
component.command_interfaces.push_back( hwi );
}
component.state_interfaces.reserve( component_info.state_interfaces.size( ) );
for ( const auto & interface : component_info.state_interfaces )
{
controller_manager_msgs::msg::HardwareInterface hwi;
hwi.name = interface;
hwi.is_available = resource_manager_->state_interface_is_available( interface );
hwi.is_claimed = false;
component.state_interfaces.push_back( hwi );
}
response->component.push_back( component );
}
RCLCPP_DEBUG( get_logger( ), "list hardware components service finished" );
}
void ControllerManager::list_hardware_interfaces_srv_cb(
const std::shared_ptr<controller_manager_msgs::srv::ListHardwareInterfaces::Request>,
std::shared_ptr<controller_manager_msgs::srv::ListHardwareInterfaces::Response> response )
{
RCLCPP_DEBUG( get_logger( ), "list hardware interfaces service called" );
std::lock_guard<std::mutex> guard( services_lock_ );
RCLCPP_DEBUG( get_logger( ), "list hardware interfaces service locked" );
auto state_interface_names = resource_manager_->state_interface_keys( );
for ( const auto & state_interface_name : state_interface_names )
{
controller_manager_msgs::msg::HardwareInterface hwi;
hwi.name = state_interface_name;
hwi.is_available = resource_manager_->state_interface_is_available( state_interface_name );
hwi.is_claimed = false;
response->state_interfaces.push_back( hwi );
}
auto command_interface_names = resource_manager_->command_interface_keys( );
for ( const auto & command_interface_name : command_interface_names )
{
controller_manager_msgs::msg::HardwareInterface hwi;
hwi.name = command_interface_name;
hwi.is_available = resource_manager_->command_interface_is_available( command_interface_name );
hwi.is_claimed = resource_manager_->command_interface_is_claimed( command_interface_name );
response->command_interfaces.push_back( hwi );
}
RCLCPP_DEBUG( get_logger( ), "list hardware interfaces service finished" );
}
void ControllerManager::set_hardware_component_state_srv_cb(
const std::shared_ptr<controller_manager_msgs::srv::SetHardwareComponentState::Request> request,
std::shared_ptr<controller_manager_msgs::srv::SetHardwareComponentState::Response> response )
{
RCLCPP_DEBUG( get_logger( ), "set hardware component state service called" );
std::lock_guard<std::mutex> guard( services_lock_ );
RCLCPP_DEBUG( get_logger( ), "set hardware component state service locked" );
RCLCPP_DEBUG( get_logger( ), "set hardware component state '%s'", request->name.c_str( ) );
auto hw_components_info = resource_manager_->get_components_status( );
if ( hw_components_info.find( request->name ) != hw_components_info.end( ) )
{
rclcpp_lifecycle::State target_state(
request->target_state.id,
// the ternary operator is needed because label in State constructor cannot be an empty string
request->target_state.label.empty( ) ? "-" : request->target_state.label );
response->ok =
( resource_manager_->set_component_state( request->name, target_state ) ==
hardware_interface::return_type::OK );
hw_components_info = resource_manager_->get_components_status( );
response->state.id = hw_components_info[request->name].state.id( );
response->state.label = hw_components_info[request->name].state.label( );
}
else
{
RCLCPP_ERROR(
get_logger( ), "hardware component with name '%s' does not exist", request->name.c_str( ) );
response->ok = false;
}
RCLCPP_DEBUG( get_logger( ), "set hardware component state service finished" );
}
std::vector<std::string> ControllerManager::get_controller_names( )
{
std::vector<std::string> names;
// lock controllers
std::lock_guard<std::recursive_mutex> guard( rt_controllers_wrapper_.controllers_lock_ );
for ( const auto & controller : rt_controllers_wrapper_.get_updated_list( guard ) )
{
names.push_back( controller.info.name );
}
return names;
}
void ControllerManager::read( const rclcpp::Time & time, const rclcpp::Duration & period )
{
resource_manager_->read( time, period );
}
controller_interface::return_type ControllerManager::update(
const rclcpp::Time & time, const rclcpp::Duration & period )
{
std::vector<ControllerSpec> & rt_controller_list =
rt_controllers_wrapper_.update_and_get_used_by_rt_list( );
auto ret = controller_interface::return_type::OK;
++update_loop_counter_;
update_loop_counter_ %= update_rate_;
for ( auto loaded_controller : rt_controller_list )
{
// TODO( v-lopez ) we could cache this information
// https://github.com/ros-controls/ros2_control/issues/153
if ( is_controller_active( *loaded_controller.c ) )
{
const auto controller_update_rate = loaded_controller.c->get_update_rate( );
bool controller_go =
controller_update_rate == 0 || ( ( update_loop_counter_ % controller_update_rate ) == 0 );
RCLCPP_DEBUG(
get_logger( ), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '",
update_loop_counter_, controller_go ? "True" : "False",
loaded_controller.info.name.c_str( ) );
if ( controller_go )
{
auto controller_ret = loaded_controller.c->update(
time, ( controller_update_rate != update_rate_ && controller_update_rate != 0 )
? rclcpp::Duration::from_seconds( 1.0 / controller_update_rate )
: period );
if ( controller_ret != controller_interface::return_type::OK )
{
ret = controller_ret;
}
}
}
}
// there are controllers to ( de )activate
if ( switch_params_.do_switch )
{
manage_switch( );
}
return ret;
}
void ControllerManager::write( const rclcpp::Time & time, const rclcpp::Duration & period )
{
resource_manager_->write( time, period );
}
std::vector<ControllerSpec> &
ControllerManager::RTControllerListWrapper::update_and_get_used_by_rt_list( )
{
used_by_realtime_controllers_index_ = updated_controllers_index_;
return controllers_lists_[used_by_realtime_controllers_index_];
}
std::vector<ControllerSpec> & ControllerManager::RTControllerListWrapper::get_unused_list(
const std::lock_guard<std::recursive_mutex> & )
{
if ( !controllers_lock_.try_lock( ) )
{
throw std::runtime_error( "controllers_lock_ not owned by thread" );
}
controllers_lock_.unlock( );
// Get the index to the outdated controller list
int free_controllers_list = get_other_list( updated_controllers_index_ );
// Wait until the outdated controller list is not being used by the realtime thread
wait_until_rt_not_using( free_controllers_list );
return controllers_lists_[free_controllers_list];
}
const std::vector<ControllerSpec> & ControllerManager::RTControllerListWrapper::get_updated_list(
const std::lock_guard<std::recursive_mutex> & ) const
{
if ( !controllers_lock_.try_lock( ) )
{
throw std::runtime_error( "controllers_lock_ not owned by thread" );
}
controllers_lock_.unlock( );
return controllers_lists_[updated_controllers_index_];
}
void ControllerManager::RTControllerListWrapper::switch_updated_list(
const std::lock_guard<std::recursive_mutex> & )
{
if ( !controllers_lock_.try_lock( ) )
{
throw std::runtime_error( "controllers_lock_ not owned by thread" );
}
controllers_lock_.unlock( );
int former_current_controllers_list_ = updated_controllers_index_;
updated_controllers_index_ = get_other_list( former_current_controllers_list_ );
wait_until_rt_not_using( former_current_controllers_list_ );
}
int ControllerManager::RTControllerListWrapper::get_other_list( int index ) const
{
return ( index + 1 ) % 2;
}
void ControllerManager::RTControllerListWrapper::wait_until_rt_not_using(
int index, std::chrono::microseconds sleep_period ) const
{
while ( used_by_realtime_controllers_index_ == index )
{
if ( !rclcpp::ok( ) )
{
throw std::runtime_error( "rclcpp interrupted" );
}
std::this_thread::sleep_for( sleep_period );
}
}
unsigned int ControllerManager::get_update_rate( ) const { return update_rate_; }
void ControllerManager::propagate_deactivation_of_chained_mode(
const std::vector<ControllerSpec> & controllers )
{
for ( const auto & controller : controllers )
{
// get pointers to places in deactivate and activate lists ( ( de )activate lists have changed )
auto deactivate_list_it =
std::find( deactivate_request_.begin( ), deactivate_request_.end( ), controller.info.name );
if ( deactivate_list_it != deactivate_request_.end( ) )
{
// if controller is not active then skip adding following-controllers to "from" chained mode
// request
if ( !is_controller_active( controller.c ) )
{
RCLCPP_DEBUG(
get_logger( ),
"Controller with name '%s' can not be deactivated since is not active. "
"The controller will be removed from the list later."
"Skipping adding following controllers to 'from' chained mode request.",
controller.info.name.c_str( ) );
break;
}
for ( const auto & cmd_itf_name : controller.c->command_interface_configuration( ).names )
{
// controller that 'cmd_tf_name' belongs to
ControllersListIterator following_ctrl_it;
if ( command_interface_is_reference_interface_of_controller(
cmd_itf_name, controllers, following_ctrl_it ) )
{
// currently iterated "controller" is preceding controller --> add following controller
// with matching interface name to "from" chained mode list ( if not already in it )
if (
std::find(
from_chained_mode_request_.begin( ), from_chained_mode_request_.end( ),
following_ctrl_it->info.name ) == from_chained_mode_request_.end( ) )
{
from_chained_mode_request_.push_back( following_ctrl_it->info.name );
RCLCPP_DEBUG(
get_logger( ), "Adding controller '%s' in 'from chained mode' request.",
following_ctrl_it->info.name.c_str( ) );
}
}
}
}
}
}
controller_interface::return_type ControllerManager::check_following_controllers_for_activate(
const std::vector<ControllerSpec> & controllers, int strictness,
const ControllersListIterator controller_it )
{
// we assume that the controller exists is checked in advance
RCLCPP_DEBUG(
get_logger( ), "Checking following controllers of preceding controller with name '%s'.",
controller_it->info.name.c_str( ) );
for ( const auto & cmd_itf_name : controller_it->c->command_interface_configuration( ).names )
{
ControllersListIterator following_ctrl_it;
// Check if interface if reference interface and following controller exist.
if ( !command_interface_is_reference_interface_of_controller(
cmd_itf_name, controllers, following_ctrl_it ) )
{
continue;
}
// TODO( destogl ): performance of this code could be optimized by adding additional lists with
// controllers that cache if the check has failed and has succeeded. Then the following would be
// done only once per controller, otherwise in complex scenarios the same controller is checked
// multiple times
// check that all following controllers exits, are either: activated, will be activated, or
// will not be deactivated
RCLCPP_DEBUG(
get_logger( ), "Checking following controller with name '%s'.",
following_ctrl_it->info.name.c_str( ) );
// check if following controller is chainable
if ( !following_ctrl_it->c->is_chainable( ) )
{
RCLCPP_WARN(
get_logger( ),
"No reference interface '%s' exist, since the following controller with name '%s' "
"is not chainable.",
cmd_itf_name.c_str( ), following_ctrl_it->info.name.c_str( ) );
return controller_interface::return_type::ERROR;
}
if ( is_controller_active( following_ctrl_it->c ) )
{
// will following controller be deactivated?
if (
std::find(
deactivate_request_.begin( ), deactivate_request_.end( ), following_ctrl_it->info.name ) !=
deactivate_request_.end( ) )
{
RCLCPP_WARN(
get_logger( ), "The following controller with name '%s' will be deactivated.",
following_ctrl_it->info.name.c_str( ) );
return controller_interface::return_type::ERROR;
}
}
// check if following controller will not be activated
else if (
std::find( activate_request_.begin( ), activate_request_.end( ), following_ctrl_it->info.name ) ==
activate_request_.end( ) )
{
RCLCPP_WARN(
get_logger( ),
"The following controller with name '%s' is not active and will not be activated.",
following_ctrl_it->info.name.c_str( ) );
return controller_interface::return_type::ERROR;
}
// Trigger recursion to check all the following controllers only if they are OK, add this
// controller update chained mode requests
if (
check_following_controllers_for_activate( controllers, strictness, following_ctrl_it ) ==
controller_interface::return_type::ERROR )
{
return controller_interface::return_type::ERROR;
}
// TODO( destogl ): this should be discussed how to it the best - just a placeholder for now
// else if ( strictness ==
// controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN )
// {
// // insert to the begin of activate request list to be activated before preceding controller
// activate_request_.insert( activate_request_.begin( ), following_ctrl_name );
// }
if ( !following_ctrl_it->c->is_in_chained_mode( ) )
{
auto found_it = std::find(
to_chained_mode_request_.begin( ), to_chained_mode_request_.end( ),
following_ctrl_it->info.name );
if ( found_it == to_chained_mode_request_.end( ) )
{
to_chained_mode_request_.push_back( following_ctrl_it->info.name );
RCLCPP_DEBUG(
get_logger( ), "Adding controller '%s' in 'to chained mode' request.",
following_ctrl_it->info.name.c_str( ) );
}
}
else
{
// Check if following controller is in 'from' chained mode list and remove it, if so
auto found_it = std::find(
from_chained_mode_request_.begin( ), from_chained_mode_request_.end( ),
following_ctrl_it->info.name );
if ( found_it != from_chained_mode_request_.end( ) )
{
from_chained_mode_request_.erase( found_it );
RCLCPP_DEBUG(
get_logger( ),
"Removing controller '%s' in 'from chained mode' request because it "
"should stay in chained mode.",
following_ctrl_it->info.name.c_str( ) );
}
}
}
return controller_interface::return_type::OK;
};
controller_interface::return_type ControllerManager::check_preceeding_controllers_for_deactivate(
const std::vector<ControllerSpec> & controllers, int /*strictness*/,
const ControllersListIterator controller_it )
{
// if not chainable no need for any checks
if ( !controller_it->c->is_chainable( ) )
{
return controller_interface::return_type::OK;
}
if ( !controller_it->c->is_in_chained_mode( ) )
{
RCLCPP_DEBUG(
get_logger( ),
"Controller with name '%s' is chainable but not in chained mode. "
"No need to do any checks of preceding controllers when stopping it.",
controller_it->info.name.c_str( ) );
return controller_interface::return_type::OK;
}
RCLCPP_DEBUG(
get_logger( ), "Checking preceding controller of following controller with name '%s'.",
controller_it->info.name.c_str( ) );
for ( const auto & ref_itf_name :
resource_manager_->get_controller_reference_interface_names( controller_it->info.name ) )
{
std::vector<ControllersListIterator> preceding_controllers_using_ref_itf;
// TODO( destogl ): This data could be cached after configuring controller into a map for faster
// access here
for ( auto preceding_ctrl_it = controllers.begin( ); preceding_ctrl_it != controllers.end( );
++preceding_ctrl_it )
{
const auto preceding_ctrl_cmd_itfs =
preceding_ctrl_it->c->command_interface_configuration( ).names;
// if controller is not preceding go the next one
if (
std::find( preceding_ctrl_cmd_itfs.begin( ), preceding_ctrl_cmd_itfs.end( ), ref_itf_name ) ==
preceding_ctrl_cmd_itfs.end( ) )
{
continue;
}
// check if preceding controller will be activated
if (
is_controller_inactive( preceding_ctrl_it->c ) &&
std::find(
activate_request_.begin( ), activate_request_.end( ), preceding_ctrl_it->info.name ) !=
activate_request_.end( ) )
{
RCLCPP_WARN(
get_logger( ),
"Could not deactivate controller with name '%s' because "
"preceding controller with name '%s' will be activated. ",
controller_it->info.name.c_str( ), preceding_ctrl_it->info.name.c_str( ) );
return controller_interface::return_type::ERROR;
}
// check if preceding controller will not be deactivated
else if (
is_controller_active( preceding_ctrl_it->c ) &&
std::find(
deactivate_request_.begin( ), deactivate_request_.end( ), preceding_ctrl_it->info.name ) ==
deactivate_request_.end( ) )
{
RCLCPP_WARN(
get_logger( ),
"Could not deactivate controller with name '%s' because "
"preceding controller with name '%s' is active and will not be deactivated.",
controller_it->info.name.c_str( ), preceding_ctrl_it->info.name.c_str( ) );
return controller_interface::return_type::ERROR;
}
// TODO( destogl ): this should be discussed how to it the best - just a placeholder for now
// else if (
// strictness ==
// controller_manager_msgs::srv::SwitchController::Request::MANIPULATE_CONTROLLERS_CHAIN )
// {
// // insert to the begin of activate request list to be activated before preceding controller
// activate_request_.insert( activate_request_.begin( ), preceding_ctrl_name );
// }
}
}
return controller_interface::return_type::OK;
};
} // namespace controller_manager
1 // Copyright 2022 PickNik Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "controller_manager/realtime.hpp"
#include <sched.h>
#include <cstring>
#include <fstream>
namespace controller_manager
{
24 bool has_realtime_kernel( )
{
std::ifstream realtime_file( "/sys/kernel/realtime", std::ios::in );
bool has_realtime = false;
if ( realtime_file.is_open( ) )
{
realtime_file >> has_realtime;
}
return has_realtime;
}
35 bool configure_sched_fifo( int priority )
{
struct sched_param schedp;
memset( &schedp, 0, sizeof( schedp ) );
schedp.sched_priority = priority;
if ( sched_setscheduler( 0, SCHED_FIFO, &schedp ) )
{
return false;
}
return true;
}
} // namespace controller_manager
1 // Copyright 2020 ROS2-Control Development Team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <algorithm>
#include <chrono>
#include <memory>
#include <string>
#include <thread>
#include "controller_manager/controller_manager.hpp"
#include "controller_manager/realtime.hpp"
#include "rclcpp/rclcpp.hpp"
using namespace std::chrono_literals;
namespace
{
// Reference: https://man7.org/linux/man-pages/man2/sched_setparam.2.html
// This value is used when configuring the main loop to use SCHED_FIFO scheduling
// We use a midpoint RT priority to allow maximum flexibility to users
32 int const kSchedPriority = 50;
} // namespace
36 int main( int argc, char ** argv )
{
rclcpp::init( argc, argv );
std::shared_ptr<rclcpp::Executor> executor =
std::make_shared<rclcpp::executors::MultiThreadedExecutor>( );
std::string manager_node_name = "controller_manager";
auto cm = std::make_shared<controller_manager::ControllerManager>( executor, manager_node_name );
RCLCPP_INFO( cm->get_logger( ), "update rate is %d Hz", cm->get_update_rate( ) );
std::thread cm_thread(
[cm]( )
{
if ( controller_manager::has_realtime_kernel( ) )
{
if ( !controller_manager::configure_sched_fifo( kSchedPriority ) )
{
RCLCPP_WARN( cm->get_logger( ), "Could not enable FIFO RT scheduling policy" );
}
}
else
{
RCLCPP_INFO( cm->get_logger( ), "RT kernel is recommended for better performance" );
}
// for calculating sleep time
auto const period = std::chrono::nanoseconds( 1'000'000'000 / cm->get_update_rate( ) );
std::chrono::system_clock::time_point next_iteration_time =
std::chrono::system_clock::time_point( std::chrono::nanoseconds( cm->now( ).nanoseconds( ) ) );
// for calculating the measured period of the loop
rclcpp::Time previous_time = cm->now( );
while ( rclcpp::ok( ) )
{
// calculate measured period
auto const current_time = cm->now( );
auto const measured_period = current_time - previous_time;
previous_time = current_time;
// execute update loop
cm->read( cm->now( ), measured_period );
cm->update( cm->now( ), measured_period );
cm->write( cm->now( ), measured_period );
// wait until we hit the end of the period
next_iteration_time += period;
std::this_thread::sleep_until( next_iteration_time );
}
} );
executor->add_node( cm );
executor->spin( );
cm_thread.join( );
rclcpp::shutdown( );
return 0;
}
// Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef CONTROLLER_MANAGER_TEST_COMMON_HPP_
#define CONTROLLER_MANAGER_TEST_COMMON_HPP_
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <chrono>
#include <memory>
#include <string>
#include <thread>
#include <vector>
#include "controller_interface/controller_interface.hpp"
#include "controller_manager/controller_manager.hpp"
#include "controller_manager_msgs/srv/switch_controller.hpp"
#include "rclcpp/utilities.hpp"
#include "ros2_control_test_assets/descriptions.hpp"
#include "test_controller_failed_init/test_controller_failed_init.hpp"
namespace
{
39 const auto TIME = rclcpp::Time( 0 );
const auto PERIOD = rclcpp::Duration::from_seconds( 0.01 );
const auto STRICT = controller_manager_msgs::srv::SwitchController::Request::STRICT;
const auto BEST_EFFORT = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT;
const auto TEST_CM_NAME = "test_controller_manager";
} // namespace
// Strictness structure for parameterized tests - shared between different tests
struct Strictness
{
int strictness = STRICT;
controller_interface::return_type expected_return;
unsigned int expected_counter;
};
Strictness strict{STRICT, controller_interface::return_type::ERROR, 0u};
Strictness best_effort{BEST_EFFORT, controller_interface::return_type::OK, 1u};
// Forward definition to avid compile error - defined at the end of the file
template <typename CtrlMgr>
class ControllerManagerRunner;
template <typename CtrlMgr>
class ControllerManagerFixture : public ::testing::Test
{
public:
static void SetUpTestCase( ) { rclcpp::init( 0, nullptr ); }
static void TearDownTestCase( ) { rclcpp::shutdown( ); }
void SetUp( )
{
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>( );
cm_ = std::make_shared<CtrlMgr>(
std::make_unique<hardware_interface::ResourceManager>(
ros2_control_test_assets::minimal_robot_urdf, true, true ),
executor_, TEST_CM_NAME );
run_updater_ = false;
}
void TearDown( ) { stopCmUpdater( ); }
void startCmUpdater( )
{
run_updater_ = true;
updater_ = std::thread(
[&]( void ) -> void
{
while ( run_updater_ )
{
cm_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
std::this_thread::sleep_for( std::chrono::milliseconds( 10 ) );
}
} );
}
void stopCmUpdater( )
{
if ( run_updater_ )
{
run_updater_ = false;
updater_.join( );
}
}
void switch_test_controllers(
const std::vector<std::string> & start_controllers,
const std::vector<std::string> & stop_controllers, const int strictness,
const std::future_status expected_future_status = std::future_status::timeout,
const controller_interface::return_type expected_return = controller_interface::return_type::OK )
{
// First activation not possible because controller not configured
auto switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, strictness, true, rclcpp::Duration( 0, 0 ) );
ASSERT_EQ( expected_future_status, switch_future.wait_for( std::chrono::milliseconds( 100 ) ) )
<< "switch_controller should be blocking until next update cycle";
ControllerManagerRunner<CtrlMgr> cm_runner( this );
EXPECT_EQ( expected_return, switch_future.get( ) );
}
std::shared_ptr<rclcpp::Executor> executor_;
std::shared_ptr<CtrlMgr> cm_;
std::thread updater_;
bool run_updater_;
};
class TestControllerManagerSrvs
: public ControllerManagerFixture<controller_manager::ControllerManager>
{
public:
TestControllerManagerSrvs( ) {}
void SetUp( ) override
{
ControllerManagerFixture::SetUp( );
SetUpSrvsCMExecutor( );
}
void SetUpSrvsCMExecutor( )
{
update_timer_ = cm_->create_wall_timer(
std::chrono::milliseconds( 10 ),
[&]( )
{
cm_->read( TIME, PERIOD );
cm_->update( TIME, PERIOD );
cm_->write( TIME, PERIOD );
} );
executor_->add_node( cm_ );
executor_spin_future_ = std::async( std::launch::async, [this]( ) -> void { executor_->spin( ); } );
// This sleep is needed to prevent a too fast test from ending before the
// executor has began to spin, which causes it to hang
std::this_thread::sleep_for( std::chrono::milliseconds( 50 ) );
}
// FIXME: This can be deleted!
void TearDown( ) override { executor_->cancel( ); }
template <typename T>
std::shared_ptr<typename T::Response> call_service_and_wait(
rclcpp::Client<T> & client, std::shared_ptr<typename T::Request> request,
rclcpp::Executor & service_executor, bool update_controller_while_spinning = false )
{
EXPECT_TRUE( client.wait_for_service( std::chrono::milliseconds( 500 ) ) );
auto result = client.async_send_request( request );
// Wait for the result.
if ( update_controller_while_spinning )
{
while ( service_executor.spin_until_future_complete( result, std::chrono::milliseconds( 50 ) ) !=
rclcpp::FutureReturnCode::SUCCESS )
{
cm_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
}
}
else
{
EXPECT_EQ(
service_executor.spin_until_future_complete( result ), rclcpp::FutureReturnCode::SUCCESS );
}
return result.get( );
}
protected:
rclcpp::TimerBase::SharedPtr update_timer_;
std::future<void> executor_spin_future_;
};
template <typename CtrlMgr>
class ControllerManagerRunner
{
public:
explicit ControllerManagerRunner( ControllerManagerFixture<CtrlMgr> * cmf ) : cmf_( cmf )
{
cmf_->startCmUpdater( );
}
~ControllerManagerRunner( ) { cmf_->stopCmUpdater( ); }
ControllerManagerFixture<CtrlMgr> * cmf_;
};
class ControllerMock : public controller_interface::ControllerInterface
{
public:
MOCK_METHOD0( update, controller_interface::return_type( void ) );
};
#endif // CONTROLLER_MANAGER_TEST_COMMON_HPP_
// Copyright ( c ) 2022, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "test_chainable_controller.hpp"
#include <memory>
#include <string>
#include <vector>
#include "lifecycle_msgs/msg/state.hpp"
namespace test_chainable_controller
{
25 TestChainableController::TestChainableController( )
: controller_interface::ChainableControllerInterface( ),
cmd_iface_cfg_{controller_interface::interface_configuration_type::NONE},
28 state_iface_cfg_{controller_interface::interface_configuration_type::NONE}
{
}
controller_interface::InterfaceConfiguration
TestChainableController::command_interface_configuration( ) const
{
if (
get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
{
return cmd_iface_cfg_;
}
else
{
throw std::runtime_error(
"Can not get command interface configuration until the controller is configured." );
}
}
controller_interface::InterfaceConfiguration
TestChainableController::state_interface_configuration( ) const
{
if (
get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
{
return state_iface_cfg_;
}
else
{
throw std::runtime_error(
"Can not get state interface configuration until the controller is configured." );
}
}
controller_interface::return_type TestChainableController::update_reference_from_subscribers( )
{
for ( size_t i = 0; i < reference_interfaces_.size( ); ++i )
{
RCLCPP_INFO(
get_node( )->get_logger( ),
"Value of reference interface '%s' before checking external input is %f",
( std::string( get_node( )->get_name( ) ) + "/" + reference_interface_names_[i] ).c_str( ),
reference_interfaces_[i] );
}
auto joint_commands = rt_command_ptr_.readFromRT( );
reference_interfaces_ = ( *joint_commands )->data;
for ( size_t i = 0; i < reference_interfaces_.size( ); ++i )
{
RCLCPP_INFO(
get_node( )->get_logger( ),
"Updated value of reference interface '%s' after applying external input is %f",
( std::string( get_node( )->get_name( ) ) + "/" + reference_interface_names_[i] ).c_str( ),
reference_interfaces_[i] );
}
return controller_interface::return_type::OK;
}
controller_interface::return_type TestChainableController::update_and_write_commands(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ )
{
++internal_counter;
for ( size_t i = 0; i < command_interfaces_.size( ); ++i )
{
command_interfaces_[i].set_value( reference_interfaces_[i] - state_interfaces_[i].get_value( ) );
}
return controller_interface::return_type::OK;
}
CallbackReturn TestChainableController::on_init( ) { return CallbackReturn::SUCCESS; }
CallbackReturn TestChainableController::on_configure(
const rclcpp_lifecycle::State & /*previous_state*/ )
{
joints_command_subscriber_ = get_node( )->create_subscription<CmdType>(
"~/commands", rclcpp::SystemDefaultsQoS( ),
[this]( const CmdType::SharedPtr msg )
{
auto joint_commands = rt_command_ptr_.readFromNonRT( );
if ( msg->data.size( ) != ( *joint_commands )->data.size( ) )
{
rt_command_ptr_.writeFromNonRT( msg );
}
else
{
RCLCPP_ERROR_THROTTLE(
get_node( )->get_logger( ), *get_node( )->get_clock( ), 1000,
"command size ( %zu ) does not match number of reference interfaces ( %zu )",
( *joint_commands )->data.size( ), reference_interfaces_.size( ) );
}
} );
auto msg = std::make_shared<CmdType>( );
msg->data.resize( reference_interfaces_.size( ) );
rt_command_ptr_.writeFromNonRT( msg );
return CallbackReturn::SUCCESS;
}
CallbackReturn TestChainableController::on_activate(
const rclcpp_lifecycle::State & /*previous_state*/ )
{
if ( !is_in_chained_mode( ) )
{
auto msg = rt_command_ptr_.readFromRT( );
( *msg )->data = reference_interfaces_;
}
return CallbackReturn::SUCCESS;
}
CallbackReturn TestChainableController::on_cleanup(
const rclcpp_lifecycle::State & /*previous_state*/ )
{
joints_command_subscriber_.reset( );
return CallbackReturn::SUCCESS;
}
std::vector<hardware_interface::CommandInterface>
TestChainableController::on_export_reference_interfaces( )
{
std::vector<hardware_interface::CommandInterface> reference_interfaces;
for ( size_t i = 0; i < reference_interface_names_.size( ); ++i )
{
reference_interfaces.push_back( hardware_interface::CommandInterface(
get_node( )->get_name( ), reference_interface_names_[i], &reference_interfaces_[i] ) );
}
return reference_interfaces;
}
void TestChainableController::set_command_interface_configuration(
const controller_interface::InterfaceConfiguration & cfg )
{
cmd_iface_cfg_ = cfg;
}
void TestChainableController::set_state_interface_configuration(
const controller_interface::InterfaceConfiguration & cfg )
{
state_iface_cfg_ = cfg;
}
void TestChainableController::set_reference_interface_names(
const std::vector<std::string> & reference_interface_names )
{
reference_interface_names_ = reference_interface_names;
reference_interfaces_.resize( reference_interface_names.size( ), 0.0 );
}
} // namespace test_chainable_controller
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(
test_chainable_controller::TestChainableController,
controller_interface::ChainableControllerInterface )
1 // Copyright ( c ) 2022, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TEST_CHAINABLE_CONTROLLER__TEST_CHAINABLE_CONTROLLER_HPP_
#define TEST_CHAINABLE_CONTROLLER__TEST_CHAINABLE_CONTROLLER_HPP_
#include <memory>
#include <string>
#include <vector>
#include "controller_interface/chainable_controller_interface.hpp"
#include "controller_manager/visibility_control.h"
#include "rclcpp/subscription.hpp"
#include "realtime_tools/realtime_buffer.h"
#include "std_msgs/msg/float64_multi_array.hpp"
namespace test_chainable_controller
{
using CmdType = std_msgs::msg::Float64MultiArray;
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
// indicating the node name under which the controller node
// is being loaded.
35 constexpr char TEST_CONTROLLER_NAME[] = "test_chainable_controller_name";
// corresponds to the name listed within the pluginlib xml
37 constexpr char TEST_CONTROLLER_CLASS_NAME[] = "controller_manager/test_chainable_controller";
38 class TestChainableController : public controller_interface::ChainableControllerInterface
{
public:
CONTROLLER_MANAGER_PUBLIC
TestChainableController( );
CONTROLLER_MANAGER_PUBLIC
virtual ~TestChainableController( ) = default;
controller_interface::InterfaceConfiguration command_interface_configuration( ) const override;
controller_interface::InterfaceConfiguration state_interface_configuration( ) const override;
CONTROLLER_MANAGER_PUBLIC
CallbackReturn on_init( ) override;
CONTROLLER_MANAGER_PUBLIC
CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state ) override;
CONTROLLER_MANAGER_PUBLIC
CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state ) override;
CONTROLLER_MANAGER_PUBLIC
CallbackReturn on_cleanup( const rclcpp_lifecycle::State & previous_state ) override;
CONTROLLER_MANAGER_PUBLIC
std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces( ) override;
controller_interface::return_type update_reference_from_subscribers( ) override;
controller_interface::return_type update_and_write_commands(
const rclcpp::Time & time, const rclcpp::Duration & period ) override;
// Testing-relevant methods
CONTROLLER_MANAGER_PUBLIC
void set_command_interface_configuration(
const controller_interface::InterfaceConfiguration & cfg );
CONTROLLER_MANAGER_PUBLIC
void set_state_interface_configuration( const controller_interface::InterfaceConfiguration & cfg );
CONTROLLER_MANAGER_PUBLIC
void set_reference_interface_names( const std::vector<std::string> & reference_interface_names );
size_t internal_counter;
controller_interface::InterfaceConfiguration cmd_iface_cfg_;
controller_interface::InterfaceConfiguration state_iface_cfg_;
std::vector<std::string> reference_interface_names_;
realtime_tools::RealtimeBuffer<std::shared_ptr<CmdType>> rt_command_ptr_;
rclcpp::Subscription<CmdType>::SharedPtr joints_command_subscriber_;
};
} // namespace test_chainable_controller
#endif // TEST_CHAINABLE_CONTROLLER__TEST_CHAINABLE_CONTROLLER_HPP_
// Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "test_controller.hpp"
#include <memory>
#include <string>
#include "lifecycle_msgs/msg/state.hpp"
namespace test_controller
{
24 TestController::TestController( )
: controller_interface::ControllerInterface( ),
cmd_iface_cfg_{controller_interface::interface_configuration_type::NONE}
{
}
controller_interface::InterfaceConfiguration TestController::command_interface_configuration( ) const
{
if (
get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
{
return cmd_iface_cfg_;
}
else
{
throw std::runtime_error(
"Can not get command interface configuration until the controller is configured." );
}
}
controller_interface::InterfaceConfiguration TestController::state_interface_configuration( ) const
{
if (
get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
{
return state_iface_cfg_;
}
else
{
throw std::runtime_error(
"Can not get state interface configuration until the controller is configured." );
}
}
controller_interface::return_type TestController::update(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ )
{
++internal_counter;
for ( size_t i = 0; i < command_interfaces_.size( ); ++i )
{
RCLCPP_INFO(
get_node( )->get_logger( ), "Setting value of command interface '%s' to %f",
command_interfaces_[i].get_name( ).c_str( ), external_commands_for_testing_[i] );
command_interfaces_[i].set_value( external_commands_for_testing_[i] );
}
return controller_interface::return_type::OK;
}
CallbackReturn TestController::on_init( ) { return CallbackReturn::SUCCESS; }
CallbackReturn TestController::on_configure( const rclcpp_lifecycle::State & /*previous_state*/ )
{
return CallbackReturn::SUCCESS;
}
CallbackReturn TestController::on_cleanup( const rclcpp_lifecycle::State & /*previous_state*/ )
{
if ( simulate_cleanup_failure )
{
return CallbackReturn::FAILURE;
}
if ( cleanup_calls )
{
( *cleanup_calls )++;
}
return CallbackReturn::SUCCESS;
}
void TestController::set_command_interface_configuration(
const controller_interface::InterfaceConfiguration & cfg )
{
cmd_iface_cfg_ = cfg;
external_commands_for_testing_.resize( cmd_iface_cfg_.names.size( ), 0.0 );
}
void TestController::set_state_interface_configuration(
const controller_interface::InterfaceConfiguration & cfg )
{
state_iface_cfg_ = cfg;
}
} // namespace test_controller
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS( test_controller::TestController, controller_interface::ControllerInterface )
1 // Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TEST_CONTROLLER__TEST_CONTROLLER_HPP_
#define TEST_CONTROLLER__TEST_CONTROLLER_HPP_
#include <memory>
#include <string>
#include <vector>
#include "controller_interface/controller_interface.hpp"
#include "controller_manager/visibility_control.h"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
namespace test_controller
{
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
// indicating the node name under which the controller node
// is being loaded.
32 constexpr char TEST_CONTROLLER_NAME[] = "test_controller_name";
// corresponds to the name listed within the pluginlib xml
34 constexpr char TEST_CONTROLLER_CLASS_NAME[] = "controller_manager/test_controller";
35 class TestController : public controller_interface::ControllerInterface
{
public:
CONTROLLER_MANAGER_PUBLIC
TestController( );
CONTROLLER_MANAGER_PUBLIC
virtual ~TestController( ) = default;
controller_interface::InterfaceConfiguration command_interface_configuration( ) const override;
controller_interface::InterfaceConfiguration state_interface_configuration( ) const override;
CONTROLLER_MANAGER_PUBLIC
controller_interface::return_type update(
const rclcpp::Time & time, const rclcpp::Duration & period ) override;
CONTROLLER_MANAGER_PUBLIC
CallbackReturn on_init( ) override;
CONTROLLER_MANAGER_PUBLIC
CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state ) override;
CONTROLLER_MANAGER_PUBLIC
CallbackReturn on_cleanup( const rclcpp_lifecycle::State & previous_state ) override;
CONTROLLER_MANAGER_PUBLIC
void set_command_interface_configuration(
const controller_interface::InterfaceConfiguration & cfg );
CONTROLLER_MANAGER_PUBLIC
void set_state_interface_configuration( const controller_interface::InterfaceConfiguration & cfg );
size_t internal_counter = 0;
bool simulate_cleanup_failure = false;
// Variable where we store when cleanup was called, pointer because the controller
// is usually destroyed after cleanup
size_t * cleanup_calls = nullptr;
controller_interface::InterfaceConfiguration cmd_iface_cfg_;
controller_interface::InterfaceConfiguration state_iface_cfg_;
std::vector<double> external_commands_for_testing_;
};
} // namespace test_controller
#endif // TEST_CONTROLLER__TEST_CONTROLLER_HPP_
1 // Copyright 2021 ros2_control development team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "test_controller_failed_init.hpp"
#include <memory>
#include <string>
#include "lifecycle_msgs/msg/transition.hpp"
namespace test_controller_failed_init
{
24 TestControllerFailedInit::TestControllerFailedInit( ) : controller_interface::ControllerInterface( )
{
}
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
29 TestControllerFailedInit::on_init( )
{
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE;
}
34 controller_interface::return_type TestControllerFailedInit::init(
35 const std::string & /* controller_name */, const std::string & /*namespace_*/,
36 const rclcpp::NodeOptions & /*node_options*/ )
{
return controller_interface::return_type::ERROR;
}
41 controller_interface::return_type TestControllerFailedInit::update(
42 const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ )
{
return controller_interface::return_type::OK;
}
} // namespace test_controller_failed_init
#include "pluginlib/class_list_macros.hpp"
51 PLUGINLIB_EXPORT_CLASS(
test_controller_failed_init::TestControllerFailedInit, controller_interface::ControllerInterface )
1 // Copyright 2021 ros2_control development team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TEST_CONTROLLER_FAILED_INIT__TEST_CONTROLLER_FAILED_INIT_HPP_
#define TEST_CONTROLLER_FAILED_INIT__TEST_CONTROLLER_FAILED_INIT_HPP_
#include <memory>
#include <string>
#include "controller_manager/controller_manager.hpp"
#include "controller_manager/visibility_control.h"
namespace test_controller_failed_init
{
// indicating the node name under which the controller node
// is being loaded.
28 constexpr char TEST_CONTROLLER_NAME[] = "test_controller_failed_init_name";
// corresponds to the name listed within the pluginlib xml
30 constexpr char TEST_CONTROLLER_FAILED_INIT_CLASS_NAME[] =
"controller_manager/test_controller_failed_init";
32 class TestControllerFailedInit : public controller_interface::ControllerInterface
{
public:
CONTROLLER_MANAGER_PUBLIC
TestControllerFailedInit( );
CONTROLLER_MANAGER_PUBLIC
virtual ~TestControllerFailedInit( ) = default;
CONTROLLER_INTERFACE_PUBLIC
controller_interface::return_type init(
const std::string & controller_name, const std::string & namespace_ = "",
const rclcpp::NodeOptions & node_options =
rclcpp::NodeOptions( )
.allow_undeclared_parameters( true )
.automatically_declare_parameters_from_overrides( true ) ) override;
controller_interface::InterfaceConfiguration command_interface_configuration( ) const override
{
return controller_interface::InterfaceConfiguration{
controller_interface::interface_configuration_type::NONE};
}
controller_interface::InterfaceConfiguration state_interface_configuration( ) const override
{
return controller_interface::InterfaceConfiguration{
controller_interface::interface_configuration_type::NONE};
}
CONTROLLER_MANAGER_PUBLIC
controller_interface::return_type update(
const rclcpp::Time & time, const rclcpp::Duration & period ) override;
CONTROLLER_MANAGER_PUBLIC
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init( ) override;
};
} // namespace test_controller_failed_init
#endif // TEST_CONTROLLER_FAILED_INIT__TEST_CONTROLLER_FAILED_INIT_HPP_
1 // Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "controller_manager/controller_manager.hpp"
#include "controller_manager_msgs/srv/list_controllers.hpp"
#include "controller_manager_test_common.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "test_controller/test_controller.hpp"
using ::testing::_;
using ::testing::Return;
30 class TestControllerManager
31 : public ControllerManagerFixture<controller_manager::ControllerManager>,
32 public testing::WithParamInterface<Strictness>
{
};
36 TEST_P( TestControllerManager, controller_lifecycle )
{
const auto test_param = GetParam( );
auto test_controller = std::make_shared<test_controller::TestController>( );
auto test_controller2 = std::make_shared<test_controller::TestController>( );
constexpr char TEST_CONTROLLER2_NAME[] = "test_controller2_name";
cm_->add_controller(
test_controller, test_controller::TEST_CONTROLLER_NAME,
test_controller::TEST_CONTROLLER_CLASS_NAME );
cm_->add_controller(
test_controller2, TEST_CONTROLLER2_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME );
EXPECT_EQ( 2u, cm_->get_loaded_controllers( ).size( ) );
EXPECT_EQ( 2, test_controller.use_count( ) );
// setup interface to claim from controllers
controller_interface::InterfaceConfiguration cmd_itfs_cfg;
cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
for ( const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES )
{
cmd_itfs_cfg.names.push_back( interface );
}
test_controller->set_command_interface_configuration( cmd_itfs_cfg );
controller_interface::InterfaceConfiguration state_itfs_cfg;
state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
for ( const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES )
{
state_itfs_cfg.names.push_back( interface );
}
for ( const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES )
{
state_itfs_cfg.names.push_back( interface );
}
test_controller->set_state_interface_configuration( state_itfs_cfg );
controller_interface::InterfaceConfiguration cmd_itfs_cfg2;
cmd_itfs_cfg2.type = controller_interface::interface_configuration_type::INDIVIDUAL;
for ( const auto & interface : ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES )
{
cmd_itfs_cfg2.names.push_back( interface );
}
test_controller2->set_command_interface_configuration( cmd_itfs_cfg2 );
controller_interface::InterfaceConfiguration state_itfs_cfg2;
state_itfs_cfg2.type = controller_interface::interface_configuration_type::ALL;
test_controller2->set_state_interface_configuration( state_itfs_cfg2 );
// Check if namespace is set correctly
RCLCPP_INFO(
rclcpp::get_logger( "test_controller_manager" ), "Controller Manager namespace is '%s'",
cm_->get_namespace( ) );
EXPECT_STREQ( cm_->get_namespace( ), "/" );
RCLCPP_INFO(
rclcpp::get_logger( "test_controller_manager" ), "Controller 1 namespace is '%s'",
test_controller->get_node( )->get_namespace( ) );
EXPECT_STREQ( test_controller->get_node( )->get_namespace( ), "/" );
RCLCPP_INFO(
rclcpp::get_logger( "test_controller_manager" ), "Controller 2 namespace is '%s'",
test_controller2->get_node( )->get_namespace( ) );
EXPECT_STREQ( test_controller2->get_node( )->get_namespace( ), "/" );
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ) );
EXPECT_EQ( 0u, test_controller->internal_counter )
<< "Update should not reach an unconfigured controller";
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state( ).id( ) );
// configure controller
cm_->configure_controller( test_controller::TEST_CONTROLLER_NAME );
cm_->configure_controller( TEST_CONTROLLER2_NAME );
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ) );
EXPECT_EQ( 0u, test_controller->internal_counter ) << "Controller is not started";
EXPECT_EQ( 0u, test_controller2->internal_counter ) << "Controller is not started";
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state( ).id( ) );
// Start controller, will take effect at the end of the update function
std::vector<std::string> start_controllers = {"fake_controller", TEST_CONTROLLER2_NAME};
std::vector<std::string> stop_controllers = {};
auto switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration( 0, 0 ) );
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ) );
EXPECT_EQ( 0u, test_controller2->internal_counter ) << "Controller is started at the end of update";
{
ControllerManagerRunner cm_runner( this );
EXPECT_EQ( test_param.expected_return, switch_future.get( ) );
}
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ) );
EXPECT_GE( test_controller2->internal_counter, test_param.expected_counter );
// Start the real test controller, will take effect at the end of the update function
start_controllers = {test_controller::TEST_CONTROLLER_NAME};
stop_controllers = {};
switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration( 0, 0 ) );
ASSERT_EQ( std::future_status::timeout, switch_future.wait_for( std::chrono::milliseconds( 100 ) ) )
<< "switch_controller should be blocking until next update cycle";
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ) );
EXPECT_EQ( 0u, test_controller->internal_counter ) << "Controller is started at the end of update";
{
ControllerManagerRunner cm_runner( this );
EXPECT_EQ( controller_interface::return_type::OK, switch_future.get( ) );
}
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state( ).id( ) );
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ) );
EXPECT_GE( test_controller->internal_counter, 1u );
auto last_internal_counter = test_controller->internal_counter;
// Stop controller, will take effect at the end of the update function
start_controllers = {};
stop_controllers = {test_controller::TEST_CONTROLLER_NAME};
switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration( 0, 0 ) );
ASSERT_EQ( std::future_status::timeout, switch_future.wait_for( std::chrono::milliseconds( 100 ) ) )
<< "switch_controller should be blocking until next update cycle";
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ) );
EXPECT_EQ( last_internal_counter + 1u, test_controller->internal_counter )
<< "Controller is stopped at the end of update, so it should have done one more update";
{
ControllerManagerRunner cm_runner( this );
EXPECT_EQ( controller_interface::return_type::OK, switch_future.get( ) );
}
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state( ).id( ) );
auto unload_future = std::async(
std::launch::async, &controller_manager::ControllerManager::unload_controller, cm_,
test_controller::TEST_CONTROLLER_NAME );
ASSERT_EQ( std::future_status::timeout, unload_future.wait_for( std::chrono::milliseconds( 100 ) ) )
<< "unload_controller should be blocking until next update cycle";
ControllerManagerRunner cm_runner( this );
EXPECT_EQ( controller_interface::return_type::OK, unload_future.get( ) );
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state( ).id( ) );
EXPECT_EQ( 1, test_controller.use_count( ) );
}
199 TEST_P( TestControllerManager, per_controller_update_rate )
{
auto strictness = GetParam( ).strictness;
auto test_controller = std::make_shared<test_controller::TestController>( );
cm_->add_controller(
test_controller, test_controller::TEST_CONTROLLER_NAME,
test_controller::TEST_CONTROLLER_CLASS_NAME );
EXPECT_EQ( 1u, cm_->get_loaded_controllers( ).size( ) );
EXPECT_EQ( 2, test_controller.use_count( ) );
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ) );
EXPECT_EQ( 0u, test_controller->internal_counter )
<< "Update should not reach an unconfigured controller";
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state( ).id( ) );
test_controller->get_node( )->set_parameter( {"update_rate", 4} );
// configure controller
cm_->configure_controller( test_controller::TEST_CONTROLLER_NAME );
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ) );
EXPECT_EQ( 0u, test_controller->internal_counter ) << "Controller is not started";
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state( ).id( ) );
// Start controller, will take effect at the end of the update function
std::vector<std::string> start_controllers = {test_controller::TEST_CONTROLLER_NAME};
std::vector<std::string> stop_controllers = {};
auto switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, strictness, true, rclcpp::Duration( 0, 0 ) );
ASSERT_EQ( std::future_status::timeout, switch_future.wait_for( std::chrono::milliseconds( 100 ) ) )
<< "switch_controller should be blocking until next update cycle";
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ) );
EXPECT_EQ( 0u, test_controller->internal_counter ) << "Controller is started at the end of update";
{
ControllerManagerRunner cm_runner( this );
EXPECT_EQ( controller_interface::return_type::OK, switch_future.get( ) );
}
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state( ).id( ) );
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ) );
EXPECT_GE( test_controller->internal_counter, 1u );
EXPECT_EQ( test_controller->get_update_rate( ), 4u );
}
256 INSTANTIATE_TEST_SUITE_P(
test_strict_best_effort, TestControllerManager, testing::Values( strict, best_effort ) );
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <memory>
#include <string>
#include <vector>
#include "controller_manager_test_common.hpp"
#include "controller_interface/controller_interface.hpp"
#include "controller_manager/controller_manager.hpp"
#include "controller_manager_msgs/srv/list_controller_types.hpp"
#include "controller_manager_msgs/srv/list_controllers.hpp"
#include "controller_manager_msgs/srv/switch_controller.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "test_controller/test_controller.hpp"
using ::testing::_;
using ::testing::Return;
using ::testing::UnorderedElementsAre;
35 TEST_F( TestControllerManagerSrvs, list_controller_types )
{
rclcpp::executors::SingleThreadedExecutor srv_executor;
rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>( "srv_client" );
srv_executor.add_node( srv_node );
rclcpp::Client<controller_manager_msgs::srv::ListControllerTypes>::SharedPtr client =
srv_node->create_client<controller_manager_msgs::srv::ListControllerTypes>(
"test_controller_manager/list_controller_types" );
auto request = std::make_shared<controller_manager_msgs::srv::ListControllerTypes::Request>( );
auto result = call_service_and_wait( *client, request, srv_executor );
// Number depends on the controllers that exist on the system
size_t controller_types = result->types.size( );
ASSERT_GE( controller_types, 1u );
ASSERT_EQ( controller_types, result->base_classes.size( ) );
ASSERT_THAT( result->types, ::testing::Contains( test_controller::TEST_CONTROLLER_CLASS_NAME ) );
ASSERT_THAT(
result->base_classes, ::testing::Contains( "controller_interface::ControllerInterface" ) );
ASSERT_THAT(
result->base_classes,
::testing::Contains( "controller_interface::ChainableControllerInterface" ) );
}
58 TEST_F( TestControllerManagerSrvs, list_controllers_srv )
{
rclcpp::executors::SingleThreadedExecutor srv_executor;
rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>( "srv_client" );
srv_executor.add_node( srv_node );
rclcpp::Client<controller_manager_msgs::srv::ListControllers>::SharedPtr client =
srv_node->create_client<controller_manager_msgs::srv::ListControllers>(
"test_controller_manager/list_controllers" );
auto request = std::make_shared<controller_manager_msgs::srv::ListControllers::Request>( );
auto result = call_service_and_wait( *client, request, srv_executor );
ASSERT_EQ( 0u, result->controller.size( ) );
auto test_controller = std::make_shared<test_controller::TestController>( );
controller_interface::InterfaceConfiguration cmd_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL,
{"joint1/position", "joint2/velocity"}};
controller_interface::InterfaceConfiguration state_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL,
{"joint1/position", "joint1/velocity", "joint2/position"}};
test_controller->set_command_interface_configuration( cmd_cfg );
test_controller->set_state_interface_configuration( state_cfg );
auto abstract_test_controller = cm_->add_controller(
test_controller, test_controller::TEST_CONTROLLER_NAME,
test_controller::TEST_CONTROLLER_CLASS_NAME );
ASSERT_EQ( 1u, cm_->get_loaded_controllers( ).size( ) );
result = call_service_and_wait( *client, request, srv_executor );
ASSERT_EQ( 1u, result->controller.size( ) );
ASSERT_EQ( test_controller::TEST_CONTROLLER_NAME, result->controller[0].name );
ASSERT_EQ( test_controller::TEST_CONTROLLER_CLASS_NAME, result->controller[0].type );
ASSERT_EQ( "unconfigured", result->controller[0].state );
ASSERT_TRUE( result->controller[0].claimed_interfaces.empty( ) );
ASSERT_TRUE( result->controller[0].required_command_interfaces.empty( ) );
ASSERT_TRUE( result->controller[0].required_state_interfaces.empty( ) );
cm_->configure_controller( test_controller::TEST_CONTROLLER_NAME );
result = call_service_and_wait( *client, request, srv_executor );
ASSERT_EQ( 1u, result->controller.size( ) );
ASSERT_EQ( "inactive", result->controller[0].state );
ASSERT_TRUE( result->controller[0].claimed_interfaces.empty( ) );
ASSERT_THAT(
result->controller[0].required_command_interfaces,
UnorderedElementsAre( "joint1/position", "joint2/velocity" ) );
ASSERT_THAT(
result->controller[0].required_state_interfaces,
UnorderedElementsAre( "joint1/position", "joint1/velocity", "joint2/position" ) );
cm_->switch_controller(
{test_controller::TEST_CONTROLLER_NAME}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration( 0, 0 ) );
result = call_service_and_wait( *client, request, srv_executor );
ASSERT_EQ( 1u, result->controller.size( ) );
ASSERT_EQ( "active", result->controller[0].state );
ASSERT_THAT(
result->controller[0].claimed_interfaces,
UnorderedElementsAre( "joint1/position", "joint2/velocity" ) );
ASSERT_THAT(
result->controller[0].required_command_interfaces,
UnorderedElementsAre( "joint1/position", "joint2/velocity" ) );
ASSERT_THAT(
result->controller[0].required_state_interfaces,
UnorderedElementsAre( "joint1/position", "joint1/velocity", "joint2/position" ) );
cm_->switch_controller(
{}, {test_controller::TEST_CONTROLLER_NAME},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration( 0, 0 ) );
result = call_service_and_wait( *client, request, srv_executor );
ASSERT_EQ( 1u, result->controller.size( ) );
ASSERT_EQ( "inactive", result->controller[0].state );
ASSERT_TRUE( result->controller[0].claimed_interfaces.empty( ) );
ASSERT_THAT(
result->controller[0].required_command_interfaces,
UnorderedElementsAre( "joint1/position", "joint2/velocity" ) );
ASSERT_THAT(
result->controller[0].required_state_interfaces,
UnorderedElementsAre( "joint1/position", "joint1/velocity", "joint2/position" ) );
cmd_cfg = {controller_interface::interface_configuration_type::ALL};
test_controller->set_command_interface_configuration( cmd_cfg );
state_cfg = {controller_interface::interface_configuration_type::ALL};
test_controller->set_state_interface_configuration( state_cfg );
cm_->switch_controller(
{test_controller::TEST_CONTROLLER_NAME}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration( 0, 0 ) );
result = call_service_and_wait( *client, request, srv_executor );
ASSERT_EQ( 1u, result->controller.size( ) );
ASSERT_EQ( "active", result->controller[0].state );
ASSERT_THAT(
result->controller[0].claimed_interfaces,
UnorderedElementsAre(
"joint2/velocity", "joint3/velocity", "joint2/max_acceleration", "configuration/max_tcp_jerk",
"joint1/position", "joint1/max_velocity" ) );
ASSERT_THAT(
result->controller[0].required_command_interfaces,
UnorderedElementsAre(
"configuration/max_tcp_jerk", "joint1/max_velocity", "joint1/position",
"joint2/max_acceleration", "joint2/velocity", "joint3/velocity" ) );
ASSERT_THAT(
result->controller[0].required_state_interfaces,
UnorderedElementsAre(
"configuration/max_tcp_jerk", "joint1/position", "joint1/some_unlisted_interface",
"joint1/velocity", "joint2/acceleration", "joint2/position", "joint2/velocity",
"joint3/acceleration", "joint3/position", "joint3/velocity", "sensor1/velocity" ) );
cm_->switch_controller(
{}, {test_controller::TEST_CONTROLLER_NAME},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration( 0, 0 ) );
result = call_service_and_wait( *client, request, srv_executor );
ASSERT_EQ( 1u, result->controller.size( ) );
ASSERT_EQ( "inactive", result->controller[0].state );
ASSERT_TRUE( result->controller[0].claimed_interfaces.empty( ) );
ASSERT_THAT(
result->controller[0].required_command_interfaces,
UnorderedElementsAre(
"configuration/max_tcp_jerk", "joint1/max_velocity", "joint1/position",
"joint2/max_acceleration", "joint2/velocity", "joint3/velocity" ) );
ASSERT_THAT(
result->controller[0].required_state_interfaces,
UnorderedElementsAre(
"configuration/max_tcp_jerk", "joint1/position", "joint1/some_unlisted_interface",
"joint1/velocity", "joint2/acceleration", "joint2/position", "joint2/velocity",
"joint3/acceleration", "joint3/position", "joint3/velocity", "sensor1/velocity" ) );
ASSERT_EQ(
controller_interface::return_type::OK,
cm_->unload_controller( test_controller::TEST_CONTROLLER_NAME ) );
result = call_service_and_wait( *client, request, srv_executor );
ASSERT_EQ( 0u, result->controller.size( ) );
}
192 TEST_F( TestControllerManagerSrvs, reload_controller_libraries_srv )
{
rclcpp::executors::SingleThreadedExecutor srv_executor;
rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>( "srv_client" );
srv_executor.add_node( srv_node );
rclcpp::Client<controller_manager_msgs::srv::ReloadControllerLibraries>::SharedPtr client =
srv_node->create_client<controller_manager_msgs::srv::ReloadControllerLibraries>(
"test_controller_manager/reload_controller_libraries" );
auto request =
std::make_shared<controller_manager_msgs::srv::ReloadControllerLibraries::Request>( );
// Reload with no controllers running
request->force_kill = false;
auto result = call_service_and_wait( *client, request, srv_executor );
ASSERT_TRUE( result->ok );
// Add a controller, but unconfigured
std::shared_ptr<test_controller::TestController> test_controller =
std::dynamic_pointer_cast<test_controller::TestController>( cm_->load_controller(
test_controller::TEST_CONTROLLER_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME ) );
// weak_ptr so the only controller shared_ptr instance is owned by the controller_manager and
// can be completely destroyed before reloading the library
std::weak_ptr<controller_interface::ControllerInterface> test_controller_weak( test_controller );
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state( ).id( ) );
ASSERT_GT( test_controller.use_count( ), 1 )
<< "Controller manager should have have a copy of this shared ptr";
size_t cleanup_calls = 0;
test_controller->cleanup_calls = &cleanup_calls;
test_controller.reset( ); // destroy our copy of the controller
request->force_kill = false;
result = call_service_and_wait( *client, request, srv_executor, true );
ASSERT_TRUE( result->ok );
// Cleanup is not called from UNCONFIGURED: https://design.ros2.org/articles/node_lifecycle.html
ASSERT_EQ( cleanup_calls, 0u );
ASSERT_EQ( test_controller.use_count( ), 0 )
<< "No more references to the controller after reloading.";
test_controller.reset( );
// Add a controller, but inactive
test_controller = std::dynamic_pointer_cast<test_controller::TestController>( cm_->load_controller(
test_controller::TEST_CONTROLLER_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME ) );
test_controller_weak = test_controller;
cm_->configure_controller( test_controller::TEST_CONTROLLER_NAME );
ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state( ).id( ) );
ASSERT_GT( test_controller.use_count( ), 1 )
<< "Controller manager should have have a copy of this shared ptr";
cleanup_calls = 0;
test_controller->cleanup_calls = &cleanup_calls;
test_controller.reset( ); // destroy our copy of the controller
request->force_kill = false;
result = call_service_and_wait( *client, request, srv_executor, true );
ASSERT_TRUE( result->ok );
ASSERT_EQ( cleanup_calls, 1u );
ASSERT_EQ( test_controller.use_count( ), 0 )
<< "No more references to the controller after reloading.";
test_controller.reset( );
test_controller = std::dynamic_pointer_cast<test_controller::TestController>( cm_->load_controller(
test_controller::TEST_CONTROLLER_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME ) );
test_controller_weak = test_controller;
cm_->configure_controller( test_controller::TEST_CONTROLLER_NAME );
// Start Controller
cm_->switch_controller(
{test_controller::TEST_CONTROLLER_NAME}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration( 0, 0 ) );
ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state( ).id( ) );
// Failed reload due to active controller
request->force_kill = false;
result = call_service_and_wait( *client, request, srv_executor );
ASSERT_FALSE( result->ok ) << "Cannot reload if controllers are running";
ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state( ).id( ) );
ASSERT_GT( test_controller.use_count( ), 1 )
<< "Controller manager should still have have a copy of "
"this shared ptr, no unloading was performed";
cleanup_calls = 0;
test_controller->cleanup_calls = &cleanup_calls;
test_controller.reset( ); // destroy our copy of the controller
// Force stop active controller
request->force_kill = true;
result = call_service_and_wait( *client, request, srv_executor, true );
ASSERT_TRUE( result->ok );
ASSERT_EQ( test_controller_weak.use_count( ), 0 )
<< "No more references to the controller after reloading.";
ASSERT_EQ( cleanup_calls, 1u )
<< "Controller should have been stopped and cleaned up with force_kill = true";
}
291 TEST_F( TestControllerManagerSrvs, load_controller_srv )
{
rclcpp::executors::SingleThreadedExecutor srv_executor;
rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>( "srv_client" );
srv_executor.add_node( srv_node );
rclcpp::Client<controller_manager_msgs::srv::LoadController>::SharedPtr client =
srv_node->create_client<controller_manager_msgs::srv::LoadController>(
"test_controller_manager/load_controller" );
auto request = std::make_shared<controller_manager_msgs::srv::LoadController::Request>( );
request->name = test_controller::TEST_CONTROLLER_NAME;
auto result = call_service_and_wait( *client, request, srv_executor );
ASSERT_FALSE( result->ok ) << "There's no param specifying the type for " << request->name;
rclcpp::Parameter controller_type_parameter(
std::string( test_controller::TEST_CONTROLLER_NAME ) + ".type",
test_controller::TEST_CONTROLLER_CLASS_NAME );
cm_->set_parameter( controller_type_parameter );
result = call_service_and_wait( *client, request, srv_executor, true );
ASSERT_TRUE( result->ok );
EXPECT_EQ( 1u, cm_->get_loaded_controllers( ).size( ) );
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
cm_->get_loaded_controllers( )[0].c->get_state( ).id( ) );
}
316 TEST_F( TestControllerManagerSrvs, unload_controller_srv )
{
rclcpp::executors::SingleThreadedExecutor srv_executor;
rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>( "srv_client" );
srv_executor.add_node( srv_node );
rclcpp::Client<controller_manager_msgs::srv::UnloadController>::SharedPtr client =
srv_node->create_client<controller_manager_msgs::srv::UnloadController>(
"test_controller_manager/unload_controller" );
auto request = std::make_shared<controller_manager_msgs::srv::UnloadController::Request>( );
request->name = test_controller::TEST_CONTROLLER_NAME;
auto result = call_service_and_wait( *client, request, srv_executor );
ASSERT_FALSE( result->ok ) << "Controller not loaded: " << request->name;
auto test_controller = std::make_shared<test_controller::TestController>( );
auto abstract_test_controller = cm_->add_controller(
test_controller, test_controller::TEST_CONTROLLER_NAME,
test_controller::TEST_CONTROLLER_CLASS_NAME );
EXPECT_EQ( 1u, cm_->get_loaded_controllers( ).size( ) );
result = call_service_and_wait( *client, request, srv_executor, true );
ASSERT_TRUE( result->ok );
EXPECT_EQ( 0u, cm_->get_loaded_controllers( ).size( ) );
}
341 TEST_F( TestControllerManagerSrvs, configure_controller_srv )
{
rclcpp::executors::SingleThreadedExecutor srv_executor;
rclcpp::Node::SharedPtr srv_node = std::make_shared<rclcpp::Node>( "srv_client" );
srv_executor.add_node( srv_node );
rclcpp::Client<controller_manager_msgs::srv::ConfigureController>::SharedPtr client =
srv_node->create_client<controller_manager_msgs::srv::ConfigureController>(
"test_controller_manager/configure_controller" );
auto request = std::make_shared<controller_manager_msgs::srv::ConfigureController::Request>( );
request->name = test_controller::TEST_CONTROLLER_NAME;
auto result = call_service_and_wait( *client, request, srv_executor );
ASSERT_FALSE( result->ok ) << "Controller not loaded: " << request->name;
auto test_controller = std::make_shared<test_controller::TestController>( );
auto abstract_test_controller = cm_->add_controller(
test_controller, test_controller::TEST_CONTROLLER_NAME,
test_controller::TEST_CONTROLLER_CLASS_NAME );
EXPECT_EQ( 1u, cm_->get_loaded_controllers( ).size( ) );
result = call_service_and_wait( *client, request, srv_executor, true );
ASSERT_TRUE( result->ok );
EXPECT_EQ( 1u, cm_->get_loaded_controllers( ).size( ) );
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
cm_->get_loaded_controllers( )[0].c->get_state( ).id( ) );
}
1 // Copyright 2021 Department of Engineering Cybernetics, NTNU.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "test_controller_with_interfaces.hpp"
#include <memory>
#include <string>
#include "lifecycle_msgs/msg/transition.hpp"
namespace test_controller_with_interfaces
{
24 TestControllerWithInterfaces::TestControllerWithInterfaces( )
: controller_interface::ControllerInterface( )
{
}
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
30 TestControllerWithInterfaces::on_init( )
{
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}
35 controller_interface::return_type TestControllerWithInterfaces::update(
36 const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ )
{
return controller_interface::return_type::OK;
}
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
42 TestControllerWithInterfaces::on_configure( const rclcpp_lifecycle::State & /*previous_state&*/ )
{
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
48 TestControllerWithInterfaces::on_cleanup( const rclcpp_lifecycle::State & /*previous_state*/ )
{
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}
} // namespace test_controller_with_interfaces
#include "pluginlib/class_list_macros.hpp"
57 PLUGINLIB_EXPORT_CLASS(
test_controller_with_interfaces::TestControllerWithInterfaces,
controller_interface::ControllerInterface )
1 // Copyright 2020 Department of Engineering Cybernetics, NTNU
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TEST_CONTROLLER_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_HPP_
#define TEST_CONTROLLER_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_HPP_
#include <memory>
#include <string>
#include "controller_interface/visibility_control.h"
#include "controller_manager/controller_manager.hpp"
namespace test_controller_with_interfaces
{
// Corresponds to the name listed within the pluginglib xml
27 constexpr char TEST_CONTROLLER_WITH_INTERFACES_CLASS_NAME[] =
"controller_manager/test_controller_with_interfaces";
// Corresponds to the command interface to claim
30 constexpr char TEST_CONTROLLER_COMMAND_INTERFACE[] = "joint2/velocity";
31 class TestControllerWithInterfaces : public controller_interface::ControllerInterface
{
public:
CONTROLLER_MANAGER_PUBLIC
TestControllerWithInterfaces( );
CONTROLLER_MANAGER_PUBLIC
virtual ~TestControllerWithInterfaces( ) = default;
controller_interface::InterfaceConfiguration command_interface_configuration( ) const override
{
return controller_interface::InterfaceConfiguration{
controller_interface::interface_configuration_type::INDIVIDUAL,
{TEST_CONTROLLER_COMMAND_INTERFACE}};
}
controller_interface::InterfaceConfiguration state_interface_configuration( ) const override
{
return controller_interface::InterfaceConfiguration{
controller_interface::interface_configuration_type::NONE};
}
CONTROLLER_MANAGER_PUBLIC
controller_interface::return_type update(
const rclcpp::Time & time, const rclcpp::Duration & period ) override;
CONTROLLER_MANAGER_PUBLIC
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init( ) override;
CONTROLLER_MANAGER_PUBLIC
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(
const rclcpp_lifecycle::State & previous_state ) override;
CONTROLLER_MANAGER_PUBLIC
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(
const rclcpp_lifecycle::State & previous_state ) override;
};
} // namespace test_controller_with_interfaces
#endif // TEST_CONTROLLER_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_HPP_
// Copyright 2022 Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "controller_interface/controller_interface.hpp"
#include "controller_manager/controller_manager.hpp"
#include "controller_manager_msgs/srv/list_controllers.hpp"
#include "controller_manager_test_common.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/parameter.hpp"
#include "test_chainable_controller/test_chainable_controller.hpp"
#include "test_controller/test_controller.hpp"
// The tests in this file are implementing example of chained-control for DiffDrive robot example:
// https://github.com/ros-controls/roadmap/blob/9f32e215a84347aee0b519cb24d081f23bbbb224/design_drafts/cascade_control.md#motivation-purpose-and-use
// The controller have the names as stated in figure, but they are simply forwarding values without
// functionality that their name would suggest
35 class TestControllerChainingWithControllerManager;
37 class TestableTestChainableController : public test_chainable_controller::TestChainableController
{
friend TestControllerChainingWithControllerManager;
41 FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers );
42 FRIEND_TEST(
43 TestControllerChainingWithControllerManager,
44 test_chained_controllers_auto_switch_to_chained_mode );
};
47 class TestableControllerManager : public controller_manager::ControllerManager
{
friend TestControllerChainingWithControllerManager;
51 FRIEND_TEST(
52 TestControllerChainingWithControllerManagerAndChainedControllersParameter,
53 test_cm_reading_chained_controllers_parameter );
54 FRIEND_TEST(
55 TestControllerChainingWithControllerManagerAndChainedControllersParameter,
56 test_cm_reading_chained_controllers_parameter_failure_group0 );
57 FRIEND_TEST(
58 TestControllerChainingWithControllerManagerAndChainedControllersParameter,
59 test_cm_reading_chained_controllers_parameter_failure_wrong_type );
60 FRIEND_TEST(
61 TestControllerChainingWithControllerManagerAndChainedControllersParameter,
62 test_cm_reading_chained_controllers_parameter_failure_duplicated_controller );
64 FRIEND_TEST( TestControllerChainingWithControllerManager, test_chained_controllers );
65 FRIEND_TEST(
66 TestControllerChainingWithControllerManager,
67 test_chained_controllers_auto_switch_to_chained_mode );
public:
70 TestableControllerManager(
71 std::unique_ptr<hardware_interface::ResourceManager> resource_manager,
72 std::shared_ptr<rclcpp::Executor> executor,
73 const std::string & manager_node_name = "controller_manager",
74 const std::string & namespace_ = "" )
: controller_manager::ControllerManager(
std::move( resource_manager ), executor, manager_node_name, namespace_ )
{
}
};
81 class TestControllerChainingWithControllerManager
82 : public ControllerManagerFixture<TestableControllerManager>,
83 public testing::WithParamInterface<Strictness>
{
public:
86 void SetUp( )
{
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>( );
cm_ = std::make_shared<TestableControllerManager>(
std::make_unique<hardware_interface::ResourceManager>(
ros2_control_test_assets::diffbot_urdf, true, true ),
executor_, TEST_CM_NAME );
run_updater_ = false;
}
96 void SetupControllers( )
{
test_param = GetParam( );
pid_left_wheel_controller = std::make_shared<TestableTestChainableController>( );
pid_right_wheel_controller = std::make_shared<TestableTestChainableController>( );
diff_drive_controller = std::make_shared<TestableTestChainableController>( );
position_tracking_controller = std::make_shared<test_controller::TestController>( );
// configure Left Wheel controller
controller_interface::InterfaceConfiguration pid_left_cmd_ifs_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_left/velocity"}};
controller_interface::InterfaceConfiguration pid_left_state_ifs_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_left/velocity"}};
pid_left_wheel_controller->set_command_interface_configuration( pid_left_cmd_ifs_cfg );
pid_left_wheel_controller->set_state_interface_configuration( pid_left_state_ifs_cfg );
pid_left_wheel_controller->set_reference_interface_names( {"velocity"} );
// configure Left Wheel controller
controller_interface::InterfaceConfiguration pid_right_cmd_ifs_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_right/velocity"}};
controller_interface::InterfaceConfiguration pid_right_state_ifs_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL, {"wheel_right/velocity"}};
pid_right_wheel_controller->set_command_interface_configuration( pid_right_cmd_ifs_cfg );
pid_right_wheel_controller->set_state_interface_configuration( pid_right_state_ifs_cfg );
pid_right_wheel_controller->set_reference_interface_names( {"velocity"} );
// configure Diff Drive controller
controller_interface::InterfaceConfiguration diff_drive_cmd_ifs_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL,
{std::string( PID_LEFT_WHEEL ) + "/velocity", std::string( PID_RIGHT_WHEEL ) + "/velocity"}};
controller_interface::InterfaceConfiguration diff_drive_state_ifs_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL,
{"wheel_left/velocity", "wheel_right/velocity"}};
diff_drive_controller->set_command_interface_configuration( diff_drive_cmd_ifs_cfg );
diff_drive_controller->set_state_interface_configuration( diff_drive_state_ifs_cfg );
diff_drive_controller->set_reference_interface_names( {"vel_x", "vel_y", "rot_z"} );
// configure Position Tracking controller
controller_interface::InterfaceConfiguration position_tracking_cmd_ifs_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL,
{std::string( DIFF_DRIVE_CONTROLLER ) + "/vel_x",
std::string( DIFF_DRIVE_CONTROLLER ) + "/vel_y"}};
// in this simple example "vel_x" == "velocity left wheel" and "vel_y" == "velocity right wheel"
position_tracking_controller->set_command_interface_configuration(
position_tracking_cmd_ifs_cfg );
}
144 void CheckIfControllersAreAddedCorrectly( )
{
EXPECT_EQ( 4u, cm_->get_loaded_controllers( ).size( ) );
EXPECT_EQ( 2, pid_left_wheel_controller.use_count( ) );
EXPECT_EQ( 2, pid_right_wheel_controller.use_count( ) );
EXPECT_EQ( 2, diff_drive_controller.use_count( ) );
EXPECT_EQ( 2, position_tracking_controller.use_count( ) );
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
pid_left_wheel_controller->get_state( ).id( ) );
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
pid_right_wheel_controller->get_state( ).id( ) );
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
diff_drive_controller->get_state( ).id( ) );
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
position_tracking_controller->get_state( ).id( ) );
}
// order or controller configuration is not important therefore we can reuse the same method
167 void ConfigureAndCheckControllers( )
{
// Store initial values of command interfaces
size_t number_of_cmd_itfs = cm_->resource_manager_->command_interface_keys( ).size( );
// configure chainable controller and check exported interfaces
cm_->configure_controller( PID_LEFT_WHEEL );
EXPECT_EQ(
pid_left_wheel_controller->get_state( ).id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
EXPECT_EQ( cm_->resource_manager_->command_interface_keys( ).size( ), number_of_cmd_itfs + 1 );
for ( const auto & interface : {"pid_left_wheel_controller/velocity"} )
{
EXPECT_TRUE( cm_->resource_manager_->command_interface_exists( interface ) );
EXPECT_FALSE( cm_->resource_manager_->command_interface_is_available( interface ) );
EXPECT_FALSE( cm_->resource_manager_->command_interface_is_claimed( interface ) );
}
cm_->configure_controller( PID_RIGHT_WHEEL );
EXPECT_EQ(
pid_right_wheel_controller->get_state( ).id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
EXPECT_EQ( cm_->resource_manager_->command_interface_keys( ).size( ), number_of_cmd_itfs + 2 );
for ( const auto & interface : {"pid_right_wheel_controller/velocity"} )
{
EXPECT_TRUE( cm_->resource_manager_->command_interface_exists( interface ) );
EXPECT_FALSE( cm_->resource_manager_->command_interface_is_available( interface ) );
EXPECT_FALSE( cm_->resource_manager_->command_interface_is_claimed( interface ) );
}
cm_->configure_controller( DIFF_DRIVE_CONTROLLER );
EXPECT_EQ(
diff_drive_controller->get_state( ).id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
EXPECT_EQ( cm_->resource_manager_->command_interface_keys( ).size( ), number_of_cmd_itfs + 5 );
for ( const auto & interface :
{"diff_drive_controller/vel_x", "diff_drive_controller/vel_y",
"diff_drive_controller/rot_z"} )
{
EXPECT_TRUE( cm_->resource_manager_->command_interface_exists( interface ) );
EXPECT_FALSE( cm_->resource_manager_->command_interface_is_available( interface ) );
EXPECT_FALSE( cm_->resource_manager_->command_interface_is_claimed( interface ) );
}
cm_->configure_controller( POSITION_TRACKING_CONTROLLER );
EXPECT_EQ(
position_tracking_controller->get_state( ).id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
EXPECT_EQ( cm_->resource_manager_->command_interface_keys( ).size( ), number_of_cmd_itfs + 5 );
}
template <
typename T, typename std::enable_if<
std::is_convertible<T *, controller_interface::ControllerInterfaceBase *>::value,
T>::type * = nullptr>
void SetToChainedModeAndMakeReferenceInterfacesAvailable(
std::shared_ptr<T> & controller, const std::string & controller_name,
const std::vector<std::string> & reference_interfaces )
{
controller->set_chained_mode( true );
EXPECT_TRUE( controller->is_in_chained_mode( ) );
// make reference interface command_interfaces available
cm_->resource_manager_->make_controller_reference_interfaces_available( controller_name );
for ( const auto & interface : reference_interfaces )
{
EXPECT_TRUE( cm_->resource_manager_->command_interface_exists( interface ) );
EXPECT_TRUE( cm_->resource_manager_->command_interface_is_available( interface ) );
EXPECT_FALSE( cm_->resource_manager_->command_interface_is_claimed( interface ) );
}
}
template <
typename T, typename std::enable_if<
std::is_convertible<T *, controller_interface::ControllerInterfaceBase *>::value,
T>::type * = nullptr>
void check_after_de_activate(
std::shared_ptr<T> & controller, const std::vector<std::string> & claimed_command_itfs,
size_t expected_internal_counter, const controller_interface::return_type expected_return,
bool deactivated, bool claimed_interfaces_from_hw = false )
{
for ( const auto & interface : claimed_command_itfs )
{
EXPECT_TRUE( cm_->resource_manager_->command_interface_exists( interface ) );
// successful xor deactivated
if ( ( expected_return == controller_interface::return_type::OK ) != deactivated )
{
EXPECT_TRUE( cm_->resource_manager_->command_interface_exists( interface ) );
EXPECT_TRUE( cm_->resource_manager_->command_interface_is_claimed( interface ) );
}
else
{
if ( claimed_interfaces_from_hw )
{
EXPECT_TRUE( cm_->resource_manager_->command_interface_is_available( interface ) );
}
else
{
EXPECT_FALSE( cm_->resource_manager_->command_interface_is_available( interface ) );
}
EXPECT_FALSE( cm_->resource_manager_->command_interface_is_claimed( interface ) );
}
}
if ( expected_internal_counter != 0 )
{
ASSERT_EQ( controller->internal_counter, expected_internal_counter );
}
}
template <
typename T, typename std::enable_if<
std::is_convertible<T *, controller_interface::ControllerInterfaceBase *>::value,
T>::type * = nullptr>
void ActivateAndCheckController(
std::shared_ptr<T> & controller, const std::string & controller_name,
const std::vector<std::string> & claimed_command_itfs, size_t expected_internal_counter = 0u,
const controller_interface::return_type expected_return = controller_interface::return_type::OK )
{
switch_test_controllers(
{controller_name}, {}, test_param.strictness, std::future_status::timeout, expected_return );
check_after_de_activate(
controller, claimed_command_itfs, expected_internal_counter, expected_return, false );
}
template <
typename T, typename std::enable_if<
std::is_convertible<T *, controller_interface::ControllerInterfaceBase *>::value,
T>::type * = nullptr>
void DeactivateAndCheckController(
std::shared_ptr<T> & controller, const std::string & controller_name,
const std::vector<std::string> & claimed_command_itfs, size_t expected_internal_counter = 0u,
const bool claimed_interfaces_from_hw = false,
const controller_interface::return_type expected_return = controller_interface::return_type::OK )
{
switch_test_controllers(
{}, {controller_name}, test_param.strictness, std::future_status::timeout, expected_return );
check_after_de_activate(
controller, claimed_command_itfs, expected_internal_counter, expected_return, true,
claimed_interfaces_from_hw );
}
void UpdateAllControllerAndCheck(
const std::vector<double> & reference, size_t exp_internal_counter_pos_ctrl )
{
// test value that could cause bad-memory access --> cleaner error during writing tests
ASSERT_EQ( reference.size( ), 2u );
position_tracking_controller->external_commands_for_testing_[0] = reference[0];
position_tracking_controller->external_commands_for_testing_[1] = reference[1];
cm_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
cm_->resource_manager_->read( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
// check if all controllers are updated
ASSERT_EQ( position_tracking_controller->internal_counter, exp_internal_counter_pos_ctrl );
ASSERT_EQ( diff_drive_controller->internal_counter, exp_internal_counter_pos_ctrl + 2 );
ASSERT_EQ( pid_left_wheel_controller->internal_counter, exp_internal_counter_pos_ctrl + 6 );
ASSERT_EQ( pid_right_wheel_controller->internal_counter, exp_internal_counter_pos_ctrl + 4 );
// check if values are set properly in controllers
ASSERT_EQ(
diff_drive_controller->reference_interfaces_[0], reference[0] ); // command from Position to
ASSERT_EQ(
diff_drive_controller->reference_interfaces_[1], reference[1] ); // DiffDrive is forwarded
// Command of DiffDrive controller are references of PID controllers
EXP_LEFT_WHEEL_REF = chained_ctrl_calculation( reference[0], EXP_LEFT_WHEEL_HW_STATE );
EXP_RIGHT_WHEEL_REF = chained_ctrl_calculation( reference[1], EXP_RIGHT_WHEEL_HW_STATE );
ASSERT_EQ( diff_drive_controller->command_interfaces_[0].get_value( ), EXP_LEFT_WHEEL_REF );
ASSERT_EQ( diff_drive_controller->command_interfaces_[1].get_value( ), EXP_RIGHT_WHEEL_REF );
ASSERT_EQ( pid_left_wheel_controller->reference_interfaces_[0], EXP_LEFT_WHEEL_REF );
ASSERT_EQ( pid_right_wheel_controller->reference_interfaces_[0], EXP_RIGHT_WHEEL_REF );
EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation( EXP_LEFT_WHEEL_REF, EXP_LEFT_WHEEL_HW_STATE );
EXP_LEFT_WHEEL_HW_STATE = hardware_calculation( EXP_LEFT_WHEEL_CMD );
ASSERT_EQ( pid_left_wheel_controller->command_interfaces_[0].get_value( ), EXP_LEFT_WHEEL_CMD );
ASSERT_EQ( pid_left_wheel_controller->state_interfaces_[0].get_value( ), EXP_LEFT_WHEEL_HW_STATE );
// DiffDrive uses the same state
ASSERT_EQ( diff_drive_controller->state_interfaces_[0].get_value( ), EXP_LEFT_WHEEL_HW_STATE );
EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation( EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE );
EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation( EXP_RIGHT_WHEEL_CMD );
ASSERT_EQ( pid_right_wheel_controller->command_interfaces_[0].get_value( ), EXP_RIGHT_WHEEL_CMD );
ASSERT_EQ(
pid_right_wheel_controller->state_interfaces_[0].get_value( ), EXP_RIGHT_WHEEL_HW_STATE );
// DiffDrive uses the same state
ASSERT_EQ( diff_drive_controller->state_interfaces_[1].get_value( ), EXP_RIGHT_WHEEL_HW_STATE );
}
// check data propagation through controllers and if individual controllers are working
double chained_ctrl_calculation( double reference, double state ) { return reference - state; }
double hardware_calculation( double command ) { return command / 2.0; }
// set controllers' names
static constexpr char PID_LEFT_WHEEL[] = "pid_left_wheel_controller";
static constexpr char PID_RIGHT_WHEEL[] = "pid_right_wheel_controller";
static constexpr char DIFF_DRIVE_CONTROLLER[] = "diff_drive_controller";
static constexpr char POSITION_TRACKING_CONTROLLER[] = "position_tracking_controller";
const std::vector<std::string> PID_LEFT_WHEEL_REFERENCE_INTERFACES = {
"pid_left_wheel_controller/velocity"};
const std::vector<std::string> PID_RIGHT_WHEEL_REFERENCE_INTERFACES = {
"pid_right_wheel_controller/velocity"};
const std::vector<std::string> DIFF_DRIVE_REFERENCE_INTERFACES = {
"diff_drive_controller/vel_x", "diff_drive_controller/vel_y", "diff_drive_controller/rot_z"};
const std::vector<std::string> PID_LEFT_WHEEL_CLAIMED_INTERFACES = {"wheel_left/velocity"};
const std::vector<std::string> PID_RIGHT_WHEEL_CLAIMED_INTERFACES = {"wheel_right/velocity"};
const std::vector<std::string> DIFF_DRIVE_CLAIMED_INTERFACES = {
"pid_left_wheel_controller/velocity", "pid_right_wheel_controller/velocity"};
const std::vector<std::string> POSITION_CONTROLLER_CLAIMED_INTERFACES = {
"diff_drive_controller/vel_x", "diff_drive_controller/vel_y"};
// controllers objects
std::shared_ptr<TestableTestChainableController> pid_left_wheel_controller;
std::shared_ptr<TestableTestChainableController> pid_right_wheel_controller;
std::shared_ptr<TestableTestChainableController> diff_drive_controller;
std::shared_ptr<test_controller::TestController> position_tracking_controller;
testing::WithParamInterface<Strictness>::ParamType test_param;
// expected values for tests - shared between multiple test runs
double EXP_LEFT_WHEEL_CMD = 0.0;
double EXP_LEFT_WHEEL_HW_STATE = 0.0;
double EXP_RIGHT_WHEEL_CMD = 0.0;
double EXP_RIGHT_WHEEL_HW_STATE = 0.0;
double EXP_LEFT_WHEEL_REF = 0.0;
double EXP_RIGHT_WHEEL_REF = 0.0;
};
// The tests are implementing example of chained-control for DiffDrive robot shown here:
// https://github.com/ros-controls/roadmap/blob/9f32e215a84347aee0b519cb24d081f23bbbb224/design_drafts/cascade_control.md#motivation-purpose-and-use
// The controller have the names as stated in figure, but they are simply forwarding values without
// functionality that their name would suggest
TEST_P( TestControllerChainingWithControllerManager, test_chained_controllers )
{
SetupControllers( );
// add all controllers - CONTROLLERS HAVE TO ADDED IN EXECUTION ORDER
cm_->add_controller(
position_tracking_controller, POSITION_TRACKING_CONTROLLER,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME );
cm_->add_controller(
diff_drive_controller, DIFF_DRIVE_CONTROLLER,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME );
cm_->add_controller(
pid_left_wheel_controller, PID_LEFT_WHEEL,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME );
cm_->add_controller(
pid_right_wheel_controller, PID_RIGHT_WHEEL,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME );
CheckIfControllersAreAddedCorrectly( );
ConfigureAndCheckControllers( );
SetToChainedModeAndMakeReferenceInterfacesAvailable(
pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_REFERENCE_INTERFACES );
SetToChainedModeAndMakeReferenceInterfacesAvailable(
pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_REFERENCE_INTERFACES );
SetToChainedModeAndMakeReferenceInterfacesAvailable(
diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_REFERENCE_INTERFACES );
EXPECT_THROW(
cm_->resource_manager_->make_controller_reference_interfaces_available(
POSITION_TRACKING_CONTROLLER ),
std::out_of_range );
// Set ControllerManager into Debug-Mode output to have detailed output on updating controllers
cm_->get_logger( ).set_level( rclcpp::Logger::Level::Debug );
// activate controllers - CONTROLLERS HAVE TO ADDED REVERSE EXECUTION ORDER
// ( otherwise, interface will be missing )
ActivateAndCheckController(
pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 1u );
ActivateAndCheckController(
pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 1u );
ASSERT_EQ( pid_left_wheel_controller->internal_counter, 3u );
// Diff-Drive Controller claims the reference interfaces of PID controllers
ActivateAndCheckController(
diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 1u );
ASSERT_EQ( pid_right_wheel_controller->internal_counter, 3u );
ASSERT_EQ( pid_left_wheel_controller->internal_counter, 5u );
// Position-Tracking Controller uses reference interfaces of Diff-Drive Controller
ActivateAndCheckController(
position_tracking_controller, POSITION_TRACKING_CONTROLLER,
POSITION_CONTROLLER_CLAIMED_INTERFACES, 1u );
// 'rot_z' reference interface is not claimed
for ( const auto & interface : {"diff_drive_controller/rot_z"} )
{
EXPECT_TRUE( cm_->resource_manager_->command_interface_exists( interface ) );
EXPECT_TRUE( cm_->resource_manager_->command_interface_is_available( interface ) );
EXPECT_FALSE( cm_->resource_manager_->command_interface_is_claimed( interface ) );
}
ASSERT_EQ( diff_drive_controller->internal_counter, 3u );
ASSERT_EQ( pid_right_wheel_controller->internal_counter, 5u );
ASSERT_EQ( pid_left_wheel_controller->internal_counter, 7u );
// update controllers
std::vector<double> reference = {32.0, 128.0};
// update 'Position Tracking' controller
for ( auto & value : diff_drive_controller->reference_interfaces_ )
{
ASSERT_EQ( value, 0.0 ); // default reference values are 0.0
}
position_tracking_controller->external_commands_for_testing_[0] = reference[0];
position_tracking_controller->external_commands_for_testing_[1] = reference[1];
position_tracking_controller->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
ASSERT_EQ( position_tracking_controller->internal_counter, 2u );
ASSERT_EQ( diff_drive_controller->reference_interfaces_[0], reference[0] ); // position_controller
ASSERT_EQ( diff_drive_controller->reference_interfaces_[1], reference[1] ); // is pass-through
// update 'Diff Drive' Controller
diff_drive_controller->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
ASSERT_EQ( diff_drive_controller->internal_counter, 4u );
// default reference values are 0.0 - they should be changed now
EXP_LEFT_WHEEL_REF = chained_ctrl_calculation( reference[0], EXP_LEFT_WHEEL_HW_STATE ); // 32-0
EXP_RIGHT_WHEEL_REF = chained_ctrl_calculation( reference[1], EXP_RIGHT_WHEEL_HW_STATE ); // 128-0
ASSERT_EQ( pid_left_wheel_controller->reference_interfaces_[0], EXP_LEFT_WHEEL_REF );
ASSERT_EQ( pid_right_wheel_controller->reference_interfaces_[0], EXP_RIGHT_WHEEL_REF );
// update PID controllers that are writing to hardware
pid_left_wheel_controller->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
ASSERT_EQ( pid_left_wheel_controller->internal_counter, 8u );
pid_right_wheel_controller->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
ASSERT_EQ( pid_right_wheel_controller->internal_counter, 6u );
// update hardware ( 'read' is sufficient for test hardware )
cm_->resource_manager_->read( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
// 32 - 0
EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation( EXP_LEFT_WHEEL_REF, EXP_LEFT_WHEEL_HW_STATE );
// 32 / 2
EXP_LEFT_WHEEL_HW_STATE = hardware_calculation( EXP_LEFT_WHEEL_CMD );
ASSERT_EQ( pid_left_wheel_controller->command_interfaces_[0].get_value( ), EXP_LEFT_WHEEL_CMD );
ASSERT_EQ( pid_left_wheel_controller->state_interfaces_[0].get_value( ), EXP_LEFT_WHEEL_HW_STATE );
// DiffDrive uses the same state
ASSERT_EQ( diff_drive_controller->state_interfaces_[0].get_value( ), EXP_LEFT_WHEEL_HW_STATE );
// 128 - 0
EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation( EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE );
// 128 / 2
EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation( EXP_RIGHT_WHEEL_CMD );
ASSERT_EQ( pid_right_wheel_controller->command_interfaces_[0].get_value( ), EXP_RIGHT_WHEEL_CMD );
ASSERT_EQ( pid_right_wheel_controller->state_interfaces_[0].get_value( ), EXP_RIGHT_WHEEL_HW_STATE );
// DiffDrive uses the same state
ASSERT_EQ( diff_drive_controller->state_interfaces_[1].get_value( ), EXP_RIGHT_WHEEL_HW_STATE );
// update all controllers at once and see that all have expected values --> also checks the order
// of controller execution
reference = {1024.0, 4096.0};
UpdateAllControllerAndCheck( reference, 3u );
// TODO( destogl ): Add here also slow disabling of controllers
// TODO( destogl ): Activate test parameter use
}
TEST_P(
TestControllerChainingWithControllerManager, test_chained_controllers_auto_switch_to_chained_mode )
{
SetupControllers( );
// add all controllers - CONTROLLERS HAVE TO ADDED IN EXECUTION ORDER
cm_->add_controller(
position_tracking_controller, POSITION_TRACKING_CONTROLLER,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME );
cm_->add_controller(
diff_drive_controller, DIFF_DRIVE_CONTROLLER,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME );
cm_->add_controller(
pid_left_wheel_controller, PID_LEFT_WHEEL,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME );
cm_->add_controller(
pid_right_wheel_controller, PID_RIGHT_WHEEL,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME );
CheckIfControllersAreAddedCorrectly( );
ConfigureAndCheckControllers( );
// Set ControllerManager into Debug-Mode output to have detailed output on updating controllers
cm_->get_logger( ).set_level( rclcpp::Logger::Level::Debug );
rclcpp::get_logger( "ControllerManager::utils" ).set_level( rclcpp::Logger::Level::Debug );
// at beginning controllers are not in chained mode
EXPECT_FALSE( pid_left_wheel_controller->is_in_chained_mode( ) );
EXPECT_FALSE( pid_right_wheel_controller->is_in_chained_mode( ) );
ASSERT_FALSE( diff_drive_controller->is_in_chained_mode( ) );
// still not in chained mode because no preceding controller is activated
ActivateAndCheckController(
pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 1u );
ActivateAndCheckController(
pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 1u );
EXPECT_FALSE( pid_left_wheel_controller->is_in_chained_mode( ) );
EXPECT_FALSE( pid_right_wheel_controller->is_in_chained_mode( ) );
ASSERT_FALSE( diff_drive_controller->is_in_chained_mode( ) );
// DiffDrive ( preceding ) controller is activated --> PID controller in chained mode
ActivateAndCheckController(
diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 1u );
EXPECT_TRUE( pid_left_wheel_controller->is_in_chained_mode( ) );
EXPECT_TRUE( pid_right_wheel_controller->is_in_chained_mode( ) );
ASSERT_FALSE( diff_drive_controller->is_in_chained_mode( ) );
// PositionController is activated --> all following controller in chained mode
ActivateAndCheckController(
position_tracking_controller, POSITION_TRACKING_CONTROLLER,
POSITION_CONTROLLER_CLAIMED_INTERFACES, 1u );
EXPECT_TRUE( pid_left_wheel_controller->is_in_chained_mode( ) );
EXPECT_TRUE( pid_right_wheel_controller->is_in_chained_mode( ) );
ASSERT_TRUE( diff_drive_controller->is_in_chained_mode( ) );
UpdateAllControllerAndCheck( {32.0, 128.0}, 2u );
UpdateAllControllerAndCheck( {1024.0, 4096.0}, 3u );
// Test switch 'from chained mode' when controllers are deactivated
// PositionController is deactivated --> DiffDrive controller is not in chained mode anymore
DeactivateAndCheckController(
position_tracking_controller, POSITION_TRACKING_CONTROLLER,
POSITION_CONTROLLER_CLAIMED_INTERFACES, 4u );
EXPECT_TRUE( pid_left_wheel_controller->is_in_chained_mode( ) );
EXPECT_TRUE( pid_right_wheel_controller->is_in_chained_mode( ) );
ASSERT_FALSE( diff_drive_controller->is_in_chained_mode( ) );
// DiffDrive ( preceding ) controller is activated --> PID controller in chained mode
DeactivateAndCheckController(
diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 8u );
EXPECT_FALSE( pid_left_wheel_controller->is_in_chained_mode( ) );
EXPECT_FALSE( pid_right_wheel_controller->is_in_chained_mode( ) );
ASSERT_FALSE( diff_drive_controller->is_in_chained_mode( ) );
// all controllers are deactivated --> chained mode is not changed
DeactivateAndCheckController(
pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 14u, true );
DeactivateAndCheckController(
pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 14u, true );
EXPECT_FALSE( pid_left_wheel_controller->is_in_chained_mode( ) );
EXPECT_FALSE( pid_right_wheel_controller->is_in_chained_mode( ) );
ASSERT_FALSE( diff_drive_controller->is_in_chained_mode( ) );
}
// TODO( destogl ): Add test case with controllers added in "random" order
// TODO( destogl ): Think about strictness and chaining controllers
// new value: "START_DOWNSTREAM_CTRLS" --> start "downstream" controllers in a controllers chain
//
INSTANTIATE_TEST_SUITE_P(
test_strict_best_effort, TestControllerChainingWithControllerManager,
testing::Values( strict, best_effort ) );
// Copyright 2022 Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <memory>
#include <string>
#include <vector>
#include "controller_manager_test_common.hpp"
#include "controller_manager/controller_manager.hpp"
#include "controller_manager_msgs/msg/hardware_component_state.hpp"
#include "controller_manager_msgs/srv/list_controllers.hpp"
#include "controller_manager_msgs/srv/set_hardware_component_state.hpp"
#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/parameter.hpp"
using ::testing::_;
using ::testing::Return;
using hardware_interface::lifecycle_state_names::ACTIVE;
using hardware_interface::lifecycle_state_names::FINALIZED;
using hardware_interface::lifecycle_state_names::INACTIVE;
using hardware_interface::lifecycle_state_names::UNCONFIGURED;
using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_CLASS_TYPE;
using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES;
using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME;
using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES;
using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_TYPE;
using ros2_control_test_assets::TEST_SENSOR_HARDWARE_CLASS_TYPE;
using ros2_control_test_assets::TEST_SENSOR_HARDWARE_COMMAND_INTERFACES;
using ros2_control_test_assets::TEST_SENSOR_HARDWARE_NAME;
using ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES;
using ros2_control_test_assets::TEST_SENSOR_HARDWARE_TYPE;
using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_CLASS_TYPE;
using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES;
using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME;
using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_STATE_INTERFACES;
using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_TYPE;
using LFC_STATE = lifecycle_msgs::msg::State;
using namespace std::chrono_literals; // NOLINT
59 class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs
{
public:
void SetUp( ) override
{
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>( );
cm_ = std::make_shared<controller_manager::ControllerManager>(
std::make_unique<hardware_interface::ResourceManager>( ), executor_, TEST_CM_NAME );
run_updater_ = false;
cm_->set_parameter(
rclcpp::Parameter( "robot_description", ros2_control_test_assets::minimal_robot_urdf ) );
cm_->set_parameter( rclcpp::Parameter(
"activate_components_on_start", std::vector<std::string>( {TEST_ACTUATOR_HARDWARE_NAME} ) ) );
73 cm_->set_parameter( rclcpp::Parameter(
"configure_components_on_start", std::vector<std::string>( {TEST_SENSOR_HARDWARE_NAME} ) ) );
std::string robot_description = "";
77 cm_->get_parameter( "robot_description", robot_description );
78 if ( robot_description.empty( ) )
{
throw std::runtime_error(
"Unable to initialize resource manager, no robot description found." );
}
84 cm_->init_resource_manager( robot_description );
SetUpSrvsCMExecutor( );
}
void check_component_fileds(
const controller_manager_msgs::msg::HardwareComponentState & component,
const std::string & name, const std::string & type, const std::string & class_type,
const uint8_t state_id, const std::string & state_label )
{
EXPECT_EQ( component.name, name );
EXPECT_EQ( component.type, type );
EXPECT_EQ( component.class_type, class_type );
EXPECT_EQ( component.state.id, state_id );
EXPECT_EQ( component.state.label, state_label );
}
void list_hardware_components_and_check(
const std::vector<uint8_t> & hw_state_ids, const std::vector<std::string> & hw_state_labels,
const std::vector<std::vector<std::vector<bool>>> & hw_itfs_available_status,
const std::vector<std::vector<std::vector<bool>>> & hw_itfs_claimed_status )
{
rclcpp::executors::SingleThreadedExecutor srv_executor;
rclcpp::Node::SharedPtr list_srv_node = std::make_shared<rclcpp::Node>( "list_srv_client" );
srv_executor.add_node( list_srv_node );
rclcpp::Client<controller_manager_msgs::srv::ListHardwareComponents>::SharedPtr list_client =
list_srv_node->create_client<controller_manager_msgs::srv::ListHardwareComponents>(
std::string( TEST_CM_NAME ) + "/list_hardware_components" );
auto request =
std::make_shared<controller_manager_msgs::srv::ListHardwareComponents::Request>( );
auto result = call_service_and_wait( *list_client, request, srv_executor );
auto check_interfaces =
[](
const std::vector<controller_manager_msgs::msg::HardwareInterface> & interfaces,
const std::vector<std::string> & interface_names,
const std::vector<bool> is_available_status, const std::vector<bool> is_claimed_status )
{
for ( auto i = 0ul; i < interfaces.size( ); ++i )
{
auto it = std::find( interface_names.begin( ), interface_names.end( ), interfaces[i].name );
EXPECT_NE( it, interface_names.end( ) );
EXPECT_EQ( interfaces[i].is_available, is_available_status[i] );
EXPECT_EQ( interfaces[i].is_claimed, is_claimed_status[i] );
}
};
for ( const auto & component : result->component )
{
if ( component.name == TEST_ACTUATOR_HARDWARE_NAME )
{
check_component_fileds(
component, TEST_ACTUATOR_HARDWARE_NAME, TEST_ACTUATOR_HARDWARE_TYPE,
TEST_ACTUATOR_HARDWARE_CLASS_TYPE, hw_state_ids[0], hw_state_labels[0] );
check_interfaces(
component.command_interfaces, TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES,
hw_itfs_available_status[0][0], hw_itfs_claimed_status[0][0] );
check_interfaces(
component.state_interfaces, TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,
hw_itfs_available_status[0][1], hw_itfs_claimed_status[0][1] );
}
if ( component.name == TEST_SENSOR_HARDWARE_NAME )
{
check_component_fileds(
component, TEST_SENSOR_HARDWARE_NAME, TEST_SENSOR_HARDWARE_TYPE,
TEST_SENSOR_HARDWARE_CLASS_TYPE, hw_state_ids[1], hw_state_labels[1] );
check_interfaces(
component.command_interfaces, TEST_SENSOR_HARDWARE_COMMAND_INTERFACES,
hw_itfs_available_status[1][0], hw_itfs_claimed_status[1][0] );
check_interfaces(
component.state_interfaces, TEST_SENSOR_HARDWARE_STATE_INTERFACES,
hw_itfs_available_status[1][1], hw_itfs_claimed_status[1][1] );
}
if ( component.name == TEST_SYSTEM_HARDWARE_NAME )
{
check_component_fileds(
component, TEST_SYSTEM_HARDWARE_NAME, TEST_SYSTEM_HARDWARE_TYPE,
TEST_SYSTEM_HARDWARE_CLASS_TYPE, hw_state_ids[2], hw_state_labels[2] );
check_interfaces(
component.command_interfaces, TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,
hw_itfs_available_status[2][0], hw_itfs_claimed_status[2][0] );
check_interfaces(
component.state_interfaces, TEST_SYSTEM_HARDWARE_STATE_INTERFACES,
hw_itfs_available_status[2][1], hw_itfs_claimed_status[2][1] );
}
}
}
bool set_hardware_component_state(
const std::string & hardware_name, const uint8_t target_state_id,
const std::string & target_state_label )
{
rclcpp::executors::SingleThreadedExecutor srv_executor;
rclcpp::Node::SharedPtr mha_srv_node = std::make_shared<rclcpp::Node>( "mha_srv_client" );
srv_executor.add_node( mha_srv_node );
rclcpp::Client<controller_manager_msgs::srv::SetHardwareComponentState>::SharedPtr mha_client =
mha_srv_node->create_client<controller_manager_msgs::srv::SetHardwareComponentState>(
std::string( TEST_CM_NAME ) + "/set_hardware_component_state" );
auto request =
std::make_shared<controller_manager_msgs::srv::SetHardwareComponentState::Request>( );
request->name = hardware_name;
request->target_state.id = target_state_id;
request->target_state.label = target_state_label;
auto result = call_service_and_wait( *mha_client, request, srv_executor );
return result->ok;
}
};
class TestControllerManagerHWManagementSrvsWithoutParams
: public TestControllerManagerHWManagementSrvs
{
public:
void SetUp( ) override
{
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>( );
cm_ = std::make_shared<controller_manager::ControllerManager>(
std::make_unique<hardware_interface::ResourceManager>( ), executor_, TEST_CM_NAME );
run_updater_ = false;
// TODO( destogl ): separate this to init_tests method where parameter can be set for each test
// separately
cm_->set_parameter(
rclcpp::Parameter( "robot_description", ros2_control_test_assets::minimal_robot_urdf ) );
std::string robot_description = "";
cm_->get_parameter( "robot_description", robot_description );
if ( robot_description.empty( ) )
{
throw std::runtime_error(
"Unable to initialize resource manager, no robot description found." );
}
cm_->init_resource_manager( robot_description );
SetUpSrvsCMExecutor( );
}
};
TEST_F( TestControllerManagerHWManagementSrvs, list_hardware_components )
{
// Default status after start:
// checks if "configure_components_on_start" and "activate_components_on_start" are correctly read
list_hardware_components_and_check(
// actuator, sensor, system
std::vector<uint8_t>(
{LFC_STATE::PRIMARY_STATE_ACTIVE, LFC_STATE::PRIMARY_STATE_INACTIVE,
LFC_STATE::PRIMARY_STATE_UNCONFIGURED} ),
std::vector<std::string>( {ACTIVE, INACTIVE, UNCONFIGURED} ),
std::vector<std::vector<std::vector<bool>>>( {
// is available
{{true, true}, {true, true, true}}, // actuator
{{}, {true}}, // sensor
{{false, false, false, false}, {false, false, false, false, false, false, false}}, // system
} ),
std::vector<std::vector<std::vector<bool>>>( {
// is claimed
{{false, false}, {false, false, false}}, // actuator
{{}, {false}}, // sensor
{{false, false, false, false}, {false, false, false, false, false, false, false}}, // system
} ) );
}
// TODO( destogl ): Add tests for testing controller starting/stopping depending on hw state
TEST_F( TestControllerManagerHWManagementSrvs, selective_activate_deactivate_components_set_state )
{
using lifecycle_msgs::msg::State;
// Default status after start
list_hardware_components_and_check(
// actuator, sensor, system
std::vector<uint8_t>(
{LFC_STATE::PRIMARY_STATE_ACTIVE, LFC_STATE::PRIMARY_STATE_INACTIVE,
LFC_STATE::PRIMARY_STATE_UNCONFIGURED} ),
std::vector<std::string>( {ACTIVE, INACTIVE, UNCONFIGURED} ),
std::vector<std::vector<std::vector<bool>>>( {
// is available
{{true, true}, {true, true, true}}, // actuator
{{}, {true}}, // sensor
{{false, false, false, false}, {false, false, false, false, false, false, false}}, // system
} ),
std::vector<std::vector<std::vector<bool>>>( {
// is claimed
{{false, false}, {false, false, false}}, // actuator
{{}, {false}}, // sensor
{{false, false, false, false}, {false, false, false, false, false, false, false}}, // system
} ) );
// Activate sensor
set_hardware_component_state( TEST_SENSOR_HARDWARE_NAME, State::PRIMARY_STATE_ACTIVE, "" );
list_hardware_components_and_check(
// actuator, sensor, system
std::vector<uint8_t>(
{LFC_STATE::PRIMARY_STATE_ACTIVE, LFC_STATE::PRIMARY_STATE_ACTIVE,
LFC_STATE::PRIMARY_STATE_UNCONFIGURED} ),
std::vector<std::string>( {ACTIVE, ACTIVE, UNCONFIGURED} ),
std::vector<std::vector<std::vector<bool>>>( {
// is available
{{true, true}, {true, true, true}}, // actuator
{{}, {true}}, // sensor
{{false, false, false, false}, {false, false, false, false, false, false, false}}, // system
} ),
std::vector<std::vector<std::vector<bool>>>( {
// is claimed
{{false, false}, {false, false, false}}, // actuator
{{}, {false}}, // sensor
{{false, false, false, false}, {false, false, false, false, false, false, false}}, // system
} ) );
// Activate system directly - it should do configure automatically; ID is determined from label
set_hardware_component_state( TEST_SYSTEM_HARDWARE_NAME, 0, ACTIVE );
list_hardware_components_and_check(
// actuator, sensor, system
std::vector<uint8_t>(
{LFC_STATE::PRIMARY_STATE_ACTIVE, LFC_STATE::PRIMARY_STATE_ACTIVE,
LFC_STATE::PRIMARY_STATE_ACTIVE} ),
std::vector<std::string>( {ACTIVE, ACTIVE, ACTIVE} ),
std::vector<std::vector<std::vector<bool>>>( {
// is available
{{true, true}, {true, true, true}}, // actuator
{{}, {true}}, // sensor
{{true, true, true, true}, {true, true, true, true, true, true, true}}, // system
} ),
std::vector<std::vector<std::vector<bool>>>( {
// is claimed
{{false, false}, {false, false, false}}, // actuator
{{}, {false}}, // sensor
{{false, false, false, false}, {false, false, false, false, false, false, false}}, // system
} ) );
// Deactivate actuator
set_hardware_component_state( TEST_ACTUATOR_HARDWARE_NAME, 0, INACTIVE );
list_hardware_components_and_check(
// actuator, sensor, system
std::vector<uint8_t>(
{LFC_STATE::PRIMARY_STATE_INACTIVE, LFC_STATE::PRIMARY_STATE_ACTIVE,
LFC_STATE::PRIMARY_STATE_ACTIVE} ),
std::vector<std::string>( {INACTIVE, ACTIVE, ACTIVE} ),
std::vector<std::vector<std::vector<bool>>>( {
// is available
{{true, true}, {true, true, true}}, // actuator
{{}, {true}}, // sensor
{{true, true, true, true}, {true, true, true, true, true, true, true}}, // system
} ),
std::vector<std::vector<std::vector<bool>>>( {
// is claimed
{{false, false}, {false, false, false}}, // actuator
{{}, {false}}, // sensor
{{false, false, false, false}, {false, false, false, false, false, false, false}}, // system
} ) );
// Double activate system
set_hardware_component_state( TEST_SYSTEM_HARDWARE_NAME, LFC_STATE::PRIMARY_STATE_ACTIVE, ACTIVE );
list_hardware_components_and_check(
// actuator, sensor, system
std::vector<uint8_t>(
{LFC_STATE::PRIMARY_STATE_INACTIVE, LFC_STATE::PRIMARY_STATE_ACTIVE,
LFC_STATE::PRIMARY_STATE_ACTIVE} ),
std::vector<std::string>( {INACTIVE, ACTIVE, ACTIVE} ),
std::vector<std::vector<std::vector<bool>>>( {
// is available
{{true, true}, {true, true, true}}, // actuator
{{}, {true}}, // sensor
{{true, true, true, true}, {true, true, true, true, true, true, true}}, // system
} ),
std::vector<std::vector<std::vector<bool>>>( {
// is claimed
{{false, false}, {false, false, false}}, // actuator
{{}, {false}}, // sensor
{{false, false, false, false}, {false, false, false, false, false, false, false}}, // system
} ) );
// Set sensor to UNCONFIGURED - inactive and cleanup transitions has to be automatically done
set_hardware_component_state(
TEST_SENSOR_HARDWARE_NAME, LFC_STATE::PRIMARY_STATE_UNCONFIGURED, UNCONFIGURED );
list_hardware_components_and_check(
// actuator, sensor, system
std::vector<uint8_t>(
{LFC_STATE::PRIMARY_STATE_INACTIVE, LFC_STATE::PRIMARY_STATE_UNCONFIGURED,
LFC_STATE::PRIMARY_STATE_ACTIVE} ),
std::vector<std::string>( {INACTIVE, UNCONFIGURED, ACTIVE} ),
std::vector<std::vector<std::vector<bool>>>( {
// is available
{{true, true}, {true, true, true}}, // actuator
{{}, {false}}, // sensor
{{true, true, true, true}, {true, true, true, true, true, true, true}}, // system
} ),
std::vector<std::vector<std::vector<bool>>>( {
// is claimed
{{false, false}, {false, false, false}}, // actuator
{{}, {false}}, // sensor
{{false, false, false, false}, {false, false, false, false, false, false, false}}, // system
} ) );
}
TEST_F( TestControllerManagerHWManagementSrvsWithoutParams, test_default_activation_of_all_hardware )
{
// "configure_components_on_start" and "activate_components_on_start" are not set ( empty )
// therefore all hardware components are active
list_hardware_components_and_check(
// actuator, sensor, system
std::vector<uint8_t>(
{LFC_STATE::PRIMARY_STATE_ACTIVE, LFC_STATE::PRIMARY_STATE_ACTIVE,
LFC_STATE::PRIMARY_STATE_ACTIVE} ),
std::vector<std::string>( {ACTIVE, ACTIVE, ACTIVE} ),
std::vector<std::vector<std::vector<bool>>>( {
// is available
{{true, true}, {true, true, true}}, // actuator
{{}, {true}}, // sensor
{{true, true, true, true}, {true, true, true, true, true, true, true}}, // system
} ),
std::vector<std::vector<std::vector<bool>>>( {
// is claimed
{{false, false}, {false, false, false}}, // actuator
{{}, {false}}, // sensor
{{false, false, false, false}, {false, false, false, false, false, false, false}}, // system
} ) );
}
1 // Copyright 2021 Department of Engineering Cybernetics, NTNU.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <memory>
#include <string>
#include <vector>
#include "controller_interface/controller_interface.hpp"
#include "controller_manager/controller_manager.hpp"
#include "controller_manager_test_common.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "test_controller_with_interfaces/test_controller_with_interfaces.hpp"
using ::testing::_;
using ::testing::Return;
30 class TestReleaseInterfaces : public ControllerManagerFixture<controller_manager::ControllerManager>
{
};
34 TEST_F( TestReleaseInterfaces, switch_controllers_same_interface )
{
std::string controller_type =
test_controller_with_interfaces::TEST_CONTROLLER_WITH_INTERFACES_CLASS_NAME;
// Load two controllers of different names
std::string controller_name1 = "test_controller1";
std::string controller_name2 = "test_controller2";
ASSERT_NO_THROW( cm_->load_controller( controller_name1, controller_type ) );
ASSERT_NO_THROW( cm_->load_controller( controller_name2, controller_type ) );
ASSERT_EQ( 2u, cm_->get_loaded_controllers( ).size( ) );
controller_manager::ControllerSpec abstract_test_controller1 = cm_->get_loaded_controllers( )[0];
controller_manager::ControllerSpec abstract_test_controller2 = cm_->get_loaded_controllers( )[1];
// Configure controllers
ASSERT_EQ( controller_interface::return_type::OK, cm_->configure_controller( controller_name1 ) );
ASSERT_EQ( controller_interface::return_type::OK, cm_->configure_controller( controller_name2 ) );
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
abstract_test_controller1.c->get_state( ).id( ) );
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
abstract_test_controller2.c->get_state( ).id( ) );
{ // Test starting the first controller
RCLCPP_INFO( cm_->get_logger( ), "Starting controller #1" );
std::vector<std::string> start_controllers = {controller_name1};
std::vector<std::string> stop_controllers = {};
auto switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, STRICT, true, rclcpp::Duration( 0, 0 ) );
ASSERT_EQ( std::future_status::timeout, switch_future.wait_for( std::chrono::milliseconds( 100 ) ) )
<< "switch_controller should be blocking until next update cycle";
ControllerManagerRunner cm_runner( this );
EXPECT_EQ( controller_interface::return_type::OK, switch_future.get( ) );
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
abstract_test_controller1.c->get_state( ).id( ) );
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
abstract_test_controller2.c->get_state( ).id( ) );
}
{ // Test starting the second controller when the first is running
// Fails as they have the same command interface
RCLCPP_INFO( cm_->get_logger( ), "Starting controller #2" );
std::vector<std::string> start_controllers = {controller_name2};
std::vector<std::string> stop_controllers = {};
auto switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, STRICT, true, rclcpp::Duration( 0, 0 ) );
ASSERT_EQ( std::future_status::timeout, switch_future.wait_for( std::chrono::milliseconds( 100 ) ) )
<< "switch_controller should be blocking until next update cycle";
ControllerManagerRunner cm_runner( this );
EXPECT_EQ( controller_interface::return_type::OK, switch_future.get( ) );
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
abstract_test_controller1.c->get_state( ).id( ) );
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
abstract_test_controller2.c->get_state( ).id( ) );
}
{ // Test stopping controller #1 and starting controller #2
RCLCPP_INFO( cm_->get_logger( ), "Stopping controller #1 and starting controller #2" );
std::vector<std::string> start_controllers = {controller_name2};
std::vector<std::string> stop_controllers = {controller_name1};
auto switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, STRICT, true, rclcpp::Duration( 0, 0 ) );
ASSERT_EQ( std::future_status::timeout, switch_future.wait_for( std::chrono::milliseconds( 100 ) ) )
<< "switch_controller should be blocking until next update cycle";
ControllerManagerRunner cm_runner( this );
EXPECT_EQ( controller_interface::return_type::OK, switch_future.get( ) );
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
abstract_test_controller1.c->get_state( ).id( ) );
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
abstract_test_controller2.c->get_state( ).id( ) );
}
{ // Test stopping controller #2 and starting controller #1
RCLCPP_INFO( cm_->get_logger( ), "Starting controller #1 and stopping controller #2" );
std::vector<std::string> start_controllers = {controller_name1};
std::vector<std::string> stop_controllers = {controller_name2};
auto switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, STRICT, true, rclcpp::Duration( 0, 0 ) );
ASSERT_EQ( std::future_status::timeout, switch_future.wait_for( std::chrono::milliseconds( 100 ) ) )
<< "switch_controller should be blocking until next update cycle";
ControllerManagerRunner cm_runner( this );
EXPECT_EQ( controller_interface::return_type::OK, switch_future.get( ) );
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
abstract_test_controller1.c->get_state( ).id( ) );
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
abstract_test_controller2.c->get_state( ).id( ) );
}
{ // Test stopping both controllers when only controller #1 is running
RCLCPP_INFO(
cm_->get_logger( ),
"Stopping both controllers ( will fail in STRICT mode as controller #2 is not running )" );
std::vector<std::string> start_controllers = {};
std::vector<std::string> stop_controllers = {controller_name1, controller_name2};
auto switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, STRICT, true, rclcpp::Duration( 0, 0 ) );
// The call to switch above will fail and the CM will not block
// ASSERT_EQ(
// std::future_status::timeout,
// switch_future.wait_for( std::chrono::milliseconds( 100 ) ) ) <<
// "switch_controller should be blocking until next update cycle";
// ControllerManagerRunner cm_runner( this );
EXPECT_EQ( controller_interface::return_type::ERROR, switch_future.get( ) );
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
abstract_test_controller1.c->get_state( ).id( ) );
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
abstract_test_controller2.c->get_state( ).id( ) );
}
{ // Test stopping both controllers when only controller #1 is running
RCLCPP_INFO(
cm_->get_logger( ),
"Stopping both controllers ( will fail in BEST EFFORT mode as controller #2 is not running )" );
std::vector<std::string> start_controllers = {};
std::vector<std::string> stop_controllers = {controller_name1, controller_name2};
auto switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, BEST_EFFORT, true, rclcpp::Duration( 0, 0 ) );
ASSERT_EQ( std::future_status::timeout, switch_future.wait_for( std::chrono::milliseconds( 100 ) ) )
<< "switch_controller should be blocking until next update cycle";
ControllerManagerRunner cm_runner( this );
EXPECT_EQ( controller_interface::return_type::OK, switch_future.get( ) );
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
abstract_test_controller1.c->get_state( ).id( ) );
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
abstract_test_controller2.c->get_state( ).id( ) );
}
{ // Test starting both controllers at the same time
RCLCPP_INFO(
cm_->get_logger( ),
"Starting both controllers at the same time ( should notify about resource conflict )" );
std::vector<std::string> start_controllers = {controller_name1, controller_name2};
std::vector<std::string> stop_controllers = {};
auto switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, STRICT, true, rclcpp::Duration( 0, 0 ) );
ASSERT_EQ( std::future_status::timeout, switch_future.wait_for( std::chrono::milliseconds( 100 ) ) )
<< "switch_controller should be blocking until next update cycle";
ControllerManagerRunner cm_runner( this );
EXPECT_EQ( controller_interface::return_type::OK, switch_future.get( ) );
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
abstract_test_controller1.c->get_state( ).id( ) );
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
abstract_test_controller2.c->get_state( ).id( ) );
}
}
// Copyright 2021 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <cstdlib>
#include <memory>
#include <string>
#include <vector>
#include "controller_interface/controller_interface.hpp"
#include "controller_manager/controller_manager.hpp"
#include "controller_manager_test_common.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "test_controller/test_controller.hpp"
using ::testing::_;
using ::testing::Return;
using namespace std::chrono_literals;
32 class TestLoadController : public ControllerManagerFixture<controller_manager::ControllerManager>
{
void SetUp( ) override
{
ControllerManagerFixture::SetUp( );
update_timer_ = cm_->create_wall_timer(
std::chrono::milliseconds( 10 ),
[&]( )
{
cm_->read( TIME, PERIOD );
cm_->update( TIME, PERIOD );
cm_->write( TIME, PERIOD );
} );
47 update_executor_ =
std::make_shared<rclcpp::executors::MultiThreadedExecutor>( rclcpp::ExecutorOptions( ), 2 );
50 update_executor_->add_node( cm_ );
update_executor_spin_future_ =
std::async( std::launch::async, [this]( ) -> void { update_executor_->spin( ); } );
// This sleep is needed to prevent a too fast test from ending before the
// executor has began to spin, which causes it to hang
std::this_thread::sleep_for( 50ms );
}
void TearDown( ) override { update_executor_->cancel( ); }
protected:
rclcpp::TimerBase::SharedPtr update_timer_;
// Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks
std::shared_ptr<rclcpp::Executor> update_executor_;
std::future<void> update_executor_spin_future_;
};
int call_spawner( const std::string extra_args )
{
std::string spawner_script = "ros2 run controller_manager spawner ";
return std::system( ( spawner_script + extra_args ).c_str( ) );
}
int call_unspawner( const std::string extra_args )
{
std::string spawner_script = "ros2 run controller_manager unspawner ";
return std::system( ( spawner_script + extra_args ).c_str( ) );
}
TEST_F( TestLoadController, spawner_with_no_arguments_errors )
{
EXPECT_NE( call_spawner( "" ), 0 ) << "Missing mandatory arguments";
}
TEST_F( TestLoadController, spawner_without_manager_errors )
{
EXPECT_NE( call_spawner( "ctrl_1" ), 0 ) << "Wrong controller manager name";
}
TEST_F( TestLoadController, spawner_without_type_parameter_or_arg_errors )
{
EXPECT_NE( call_spawner( "ctrl_1 -c test_controller_manager" ), 0 ) << "Missing .type parameter";
}
TEST_F( TestLoadController, spawner_test_type_in_param )
{
cm_->set_parameter( rclcpp::Parameter( "ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME ) );
EXPECT_EQ( call_spawner( "ctrl_1 -c test_controller_manager" ), 0 );
ASSERT_EQ( cm_->get_loaded_controllers( ).size( ), 1ul );
auto ctrl_1 = cm_->get_loaded_controllers( )[0];
ASSERT_EQ( ctrl_1.info.name, "ctrl_1" );
ASSERT_EQ( ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME );
ASSERT_EQ( ctrl_1.c->get_state( ).id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
// Try to spawn again, it should fail because already active
EXPECT_NE( call_spawner( "ctrl_1 -c test_controller_manager" ), 0 ) << "Cannot configure from active";
ctrl_1.c->get_node( )->deactivate( );
// We should be able to reconfigure and activate a configured controller
EXPECT_EQ( call_spawner( "ctrl_1 -c test_controller_manager" ), 0 );
ctrl_1.c->get_node( )->deactivate( );
ctrl_1.c->get_node( )->cleanup( );
// We should be able to reconfigure and activate am unconfigured loaded controller
EXPECT_EQ( call_spawner( "ctrl_1 -c test_controller_manager" ), 0 );
ASSERT_EQ( ctrl_1.c->get_state( ).id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
// Unload and reload
EXPECT_EQ( call_unspawner( "ctrl_1 -c test_controller_manager" ), 0 );
ASSERT_EQ( cm_->get_loaded_controllers( ).size( ), 0ul ) << "Controller should have been unloaded";
EXPECT_EQ( call_spawner( "ctrl_1 -c test_controller_manager" ), 0 );
ASSERT_EQ( cm_->get_loaded_controllers( ).size( ), 1ul ) << "Controller should have been loaded";
ctrl_1 = cm_->get_loaded_controllers( )[0];
ASSERT_EQ( ctrl_1.c->get_state( ).id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
}
TEST_F( TestLoadController, spawner_test_type_in_arg )
{
// Provide controller type via -t argument
EXPECT_EQ(
call_spawner(
"ctrl_2 -c test_controller_manager -t " +
std::string( test_controller::TEST_CONTROLLER_CLASS_NAME ) ),
0 );
ASSERT_EQ( cm_->get_loaded_controllers( ).size( ), 1ul );
auto ctrl_2 = cm_->get_loaded_controllers( )[0];
ASSERT_EQ( ctrl_2.info.name, "ctrl_2" );
ASSERT_EQ( ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME );
ASSERT_EQ( ctrl_2.c->get_state( ).id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
}
TEST_F( TestLoadController, unload_on_kill )
{
// Launch spawner with unload on kill
// timeout command will kill it after the specified time with signal SIGINT
std::stringstream ss;
ss << "timeout --signal=INT 5 "
<< "ros2 run controller_manager spawner "
<< "ctrl_3 -c test_controller_manager -t "
<< std::string( test_controller::TEST_CONTROLLER_CLASS_NAME ) << " --unload-on-kill";
EXPECT_NE( std::system( ss.str( ).c_str( ) ), 0 )
<< "timeout should have killed spawner and returned non 0 code";
ASSERT_EQ( cm_->get_loaded_controllers( ).size( ), 0ul );
}
1 // Copyright ( c ) 2021 PickNik, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef FAKE_COMPONENTS__VISIBILITY_CONTROL_H_
#define FAKE_COMPONENTS__VISIBILITY_CONTROL_H_
// This logic was borrowed ( then namespaced ) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define FAKE_COMPONENTS_EXPORT __attribute__( ( dllexport ) )
#define FAKE_COMPONENTS_IMPORT __attribute__( ( dllimport ) )
#else
#define FAKE_COMPONENTS_EXPORT __declspec( dllexport )
#define FAKE_COMPONENTS_IMPORT __declspec( dllimport )
#endif
#ifdef FAKE_COMPONENTS_BUILDING_DLL
#define FAKE_COMPONENTS_PUBLIC FAKE_COMPONENTS_EXPORT
#else
#define FAKE_COMPONENTS_PUBLIC FAKE_COMPONENTS_IMPORT
#endif
#define FAKE_COMPONENTS_PUBLIC_TYPE FAKE_COMPONENTS_PUBLIC
#define FAKE_COMPONENTS_LOCAL
#else
#define FAKE_COMPONENTS_EXPORT __attribute__( ( visibility( "default" ) ) )
#define FAKE_COMPONENTS_IMPORT
#if __GNUC__ >= 4
#define FAKE_COMPONENTS_PUBLIC __attribute__( ( visibility( "default" ) ) )
#define FAKE_COMPONENTS_LOCAL __attribute__( ( visibility( "hidden" ) ) )
#else
#define FAKE_COMPONENTS_PUBLIC
#define FAKE_COMPONENTS_LOCAL
#endif
#define FAKE_COMPONENTS_PUBLIC_TYPE
#endif
#endif // FAKE_COMPONENTS__VISIBILITY_CONTROL_H_
// Copyright 2020 - 2021 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef HARDWARE_INTERFACE__ACTUATOR_HPP_
#define HARDWARE_INTERFACE__ACTUATOR_HPP_
#include <memory>
#include <string>
#include <vector>
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/visibility_control.h"
#include "rclcpp/duration.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp_lifecycle/state.hpp"
namespace hardware_interface
{
32 class ActuatorInterface;
class Actuator final
{
public:
37 Actuator( ) = default;
HARDWARE_INTERFACE_PUBLIC
explicit Actuator( std::unique_ptr<ActuatorInterface> impl );
42 Actuator( Actuator && other ) = default;
44 ~Actuator( ) = default;
46 HARDWARE_INTERFACE_PUBLIC
const rclcpp_lifecycle::State & initialize( const HardwareInfo & actuator_info );
49 HARDWARE_INTERFACE_PUBLIC
const rclcpp_lifecycle::State & configure( );
HARDWARE_INTERFACE_PUBLIC
53 const rclcpp_lifecycle::State & cleanup( );
55 HARDWARE_INTERFACE_PUBLIC
const rclcpp_lifecycle::State & shutdown( );
58 HARDWARE_INTERFACE_PUBLIC
const rclcpp_lifecycle::State & activate( );
HARDWARE_INTERFACE_PUBLIC
62 const rclcpp_lifecycle::State & deactivate( );
64 HARDWARE_INTERFACE_PUBLIC
const rclcpp_lifecycle::State & error( );
HARDWARE_INTERFACE_PUBLIC
std::vector<StateInterface> export_state_interfaces( );
HARDWARE_INTERFACE_PUBLIC
std::vector<CommandInterface> export_command_interfaces( );
HARDWARE_INTERFACE_PUBLIC
return_type prepare_command_mode_switch(
const std::vector<std::string> & start_interfaces,
const std::vector<std::string> & stop_interfaces );
HARDWARE_INTERFACE_PUBLIC
return_type perform_command_mode_switch(
const std::vector<std::string> & start_interfaces,
const std::vector<std::string> & stop_interfaces );
HARDWARE_INTERFACE_PUBLIC
84 std::string get_name( ) const;
HARDWARE_INTERFACE_PUBLIC
87 const rclcpp_lifecycle::State & get_state( ) const;
HARDWARE_INTERFACE_PUBLIC
return_type read( const rclcpp::Time & time, const rclcpp::Duration & period );
HARDWARE_INTERFACE_PUBLIC
return_type write( const rclcpp::Time & time, const rclcpp::Duration & period );
private:
std::unique_ptr<ActuatorInterface> impl_;
};
} // namespace hardware_interface
#endif // HARDWARE_INTERFACE__ACTUATOR_HPP_
// Copyright 2020 - 2021 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_
#define HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_
#include <memory>
#include <string>
#include <vector>
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/duration.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
namespace hardware_interface
{
/// Virtual Class to implement when integrating a 1 DoF actuator into ros2_control.
/**
* The typical examples are conveyors or motors.
*
* Methods return values have type rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
* with the following meaning:
*
* \returns CallbackReturn::SUCCESS method execution was successful.
* \returns CallbackReturn::FAILURE method execution has failed and and can be called again.
* \returns CallbackReturn::ERROR critical error has happened that should be managed in
* "on_error" method.
*
* The hardware ends after each method in a state with the following meaning:
*
* UNCONFIGURED ( on_init, on_cleanup ):
* Hardware is initialized but communication is not started and therefore no interface is available.
*
* INACTIVE ( on_configure, on_deactivate ):
* Communication with the hardware is started and it is configured.
* States can be read and non-movement hardware interfaces commanded.
* Hardware interfaces for movement will NOT be available.
* Those interfaces are: HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_ACCELERATION, and HW_IF_EFFORT.
*
* FINALIZED ( on_shutdown ):
* Hardware interface is ready for unloading/destruction.
* Allocated memory is cleaned up.
*
* ACTIVE ( on_activate ):
* Power circuits of hardware are active and hardware can be moved, e.g., brakes are disabled.
* Command interfaces for movement are available and have to be accepted.
* Those interfaces are: HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_ACCELERATION, and HW_IF_EFFORT.
*/
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
69 class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
{
public:
72 ActuatorInterface( )
: lifecycle_state_( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_state_names::UNKNOWN ) )
{
}
/// ActuatorInterface copy constructor is actively deleted.
/**
* Hardware interfaces are having a unique ownership and thus can't be copied in order to avoid
* failed or simultaneous access to hardware.
*/
ActuatorInterface( const ActuatorInterface & other ) = delete;
ActuatorInterface( ActuatorInterface && other ) = default;
virtual ~ActuatorInterface( ) = default;
/// Initialization of the hardware interface from data parsed from the robot's URDF.
/**
* \param[in] hardware_info structure with data from URDF.
* \returns CallbackReturn::SUCCESS if required data are provided and can be parsed.
* \returns CallbackReturn::ERROR if any error happens or data are missing.
*/
virtual CallbackReturn on_init( const HardwareInfo & hardware_info )
{
info_ = hardware_info;
return CallbackReturn::SUCCESS;
};
/// Exports all state interfaces for this hardware interface.
/**
* The state interfaces have to be created and transferred according
* to the hardware info passed in for the configuration.
*
* Note the ownership over the state interfaces is transferred to the caller.
*
* \return vector of state interfaces
*/
110 virtual std::vector<StateInterface> export_state_interfaces( ) = 0;
/// Exports all command interfaces for this hardware interface.
/**
* The command interfaces have to be created and transferred according
* to the hardware info passed in for the configuration.
*
* Note the ownership over the state interfaces is transferred to the caller.
*
* \return vector of command interfaces
*/
virtual std::vector<CommandInterface> export_command_interfaces( ) = 0;
/// Prepare for a new command interface switch.
/**
* Prepare for any mode-switching required by the new command interface combination.
*
* \note This is a non-realtime evaluation of whether a set of command interface claims are
* possible, and call to start preparing data structures for the upcoming switch that will occur.
* \note All starting and stopping interface keys are passed to all components, so the function should
* return return_type::OK by default when given interface keys not relevant for this component.
* \param[in] start_interfaces vector of string identifiers for the command interfaces starting.
* \param[in] stop_interfaces vector of string identifiers for the command interfacs stopping.
* \return return_type::OK if the new command interface combination can be prepared,
* or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise.
*/
virtual return_type prepare_command_mode_switch(
const std::vector<std::string> & /*start_interfaces*/,
const std::vector<std::string> & /*stop_interfaces*/ )
{
return return_type::OK;
}
// Perform switching to the new command interface.
/**
* Perform the mode-switching for the new command interface combination.
*
* \note This is part of the realtime update loop, and should be fast.
* \note All starting and stopping interface keys are passed to all components, so the function should
* return return_type::OK by default when given interface keys not relevant for this component.
* \param[in] start_interfaces vector of string identifiers for the command interfaces starting.
* \param[in] stop_interfaces vector of string identifiers for the command interfacs stopping.
* \return return_type::OK if the new command interface combination can be switched to,
* or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise.
*/
virtual return_type perform_command_mode_switch(
const std::vector<std::string> & /*start_interfaces*/,
const std::vector<std::string> & /*stop_interfaces*/ )
{
return return_type::OK;
}
/// Read the current state values from the actuator.
/**
* The data readings from the physical hardware has to be updated
* and reflected accordingly in the exported state interfaces.
* That is, the data pointed by the interfaces shall be updated.
*
* \return return_type::OK if the read was successful, return_type::ERROR otherwise.
*/
virtual return_type read( const rclcpp::Time & time, const rclcpp::Duration & period ) = 0;
/// Write the current command values to the actuator.
/**
* The physical hardware shall be updated with the latest value from
* the exported command interfaces.
*
* \return return_type::OK if the read was successful, return_type::ERROR otherwise.
*/
virtual return_type write( const rclcpp::Time & time, const rclcpp::Duration & period ) = 0;
/// Get name of the actuator hardware.
/**
* \return name.
*/
virtual std::string get_name( ) const { return info_.name; }
/// Get life-cycle state of the actuator hardware.
/**
* \return state.
*/
const rclcpp_lifecycle::State & get_state( ) const { return lifecycle_state_; }
/// Set life-cycle state of the actuator hardware.
/**
* \return state.
*/
void set_state( const rclcpp_lifecycle::State & new_state ) { lifecycle_state_ = new_state; }
protected:
HardwareInfo info_;
rclcpp_lifecycle::State lifecycle_state_;
};
} // namespace hardware_interface
#endif // HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_
1 // Copyright 2020 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_
#define HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_
#include <string>
#include <unordered_map>
#include <vector>
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/visibility_control.h"
namespace hardware_interface
{
/// Search XML snippet from URDF for information about a control component.
/**
* \param[in] urdf string with robot's URDF
* \return vector filled with information about robot's control resources
* \throws std::runtime_error if a robot attribute or tag is not found
*/
HARDWARE_INTERFACE_PUBLIC
std::vector<HardwareInfo> parse_control_resources_from_urdf( const std::string & urdf );
} // namespace hardware_interface
#endif // HARDWARE_INTERFACE__COMPONENT_PARSER_HPP_
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef HARDWARE_INTERFACE__CONTROLLER_INFO_HPP_
#define HARDWARE_INTERFACE__CONTROLLER_INFO_HPP_
#include <string>
#include <vector>
namespace hardware_interface
{
/// Controller Information
/**
* This struct contains information about a given controller.
*/
struct ControllerInfo
{
/// Controller name.
std::string name;
/// Controller type.
std::string type;
/// List of claimed interfaces by the controller.
std::vector<std::string> claimed_interfaces;
};
} // namespace hardware_interface
#endif // HARDWARE_INTERFACE__CONTROLLER_INFO_HPP_
// Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef HARDWARE_INTERFACE__HANDLE_HPP_
#define HARDWARE_INTERFACE__HANDLE_HPP_
#include <string>
#include <utility>
#include "hardware_interface/macros.hpp"
#include "hardware_interface/visibility_control.h"
namespace hardware_interface
{
/// A handle used to get and set a value on a given interface.
27 class ReadOnlyHandle
{
public:
30 ReadOnlyHandle(
const std::string & prefix_name, const std::string & interface_name,
double * value_ptr = nullptr )
: prefix_name_( prefix_name ), interface_name_( interface_name ), value_ptr_( value_ptr )
{
}
37 explicit ReadOnlyHandle( const std::string & interface_name )
: interface_name_( interface_name ), value_ptr_( nullptr )
{
}
42 explicit ReadOnlyHandle( const char * interface_name )
: interface_name_( interface_name ), value_ptr_( nullptr )
{
}
ReadOnlyHandle( const ReadOnlyHandle & other ) = default;
ReadOnlyHandle( ReadOnlyHandle && other ) = default;
ReadOnlyHandle & operator=( const ReadOnlyHandle & other ) = default;
ReadOnlyHandle & operator=( ReadOnlyHandle && other ) = default;
virtual ~ReadOnlyHandle( ) = default;
/// Returns true if handle references a value.
inline operator bool( ) const { return value_ptr_ != nullptr; }
const std::string get_name( ) const { return prefix_name_ + "/" + interface_name_; }
62 const std::string & get_interface_name( ) const { return interface_name_; }
[[deprecated(
"Replaced by get_name method, which is semantically more correct" )]] const std::string
get_full_name( ) const
{
return get_name( );
}
const std::string & get_prefix_name( ) const { return prefix_name_; }
double get_value( ) const
{
THROW_ON_NULLPTR( value_ptr_ );
return *value_ptr_;
}
protected:
std::string prefix_name_;
std::string interface_name_;
double * value_ptr_;
};
class ReadWriteHandle : public ReadOnlyHandle
{
public:
ReadWriteHandle(
const std::string & prefix_name, const std::string & interface_name,
double * value_ptr = nullptr )
: ReadOnlyHandle( prefix_name, interface_name, value_ptr )
{
}
explicit ReadWriteHandle( const std::string & interface_name ) : ReadOnlyHandle( interface_name ) {}
explicit ReadWriteHandle( const char * interface_name ) : ReadOnlyHandle( interface_name ) {}
ReadWriteHandle( const ReadWriteHandle & other ) = default;
ReadWriteHandle( ReadWriteHandle && other ) = default;
ReadWriteHandle & operator=( const ReadWriteHandle & other ) = default;
ReadWriteHandle & operator=( ReadWriteHandle && other ) = default;
virtual ~ReadWriteHandle( ) = default;
void set_value( double value )
{
THROW_ON_NULLPTR( this->value_ptr_ );
*this->value_ptr_ = value;
}
};
class StateInterface : public ReadOnlyHandle
{
public:
StateInterface( const StateInterface & other ) = default;
StateInterface( StateInterface && other ) = default;
using ReadOnlyHandle::ReadOnlyHandle;
};
class CommandInterface : public ReadWriteHandle
{
public:
/// CommandInterface copy constructor is actively deleted.
/**
* Command interfaces are having a unique ownership and thus
* can't be copied in order to avoid simultaneous writes to
* the same resource.
*/
CommandInterface( const CommandInterface & other ) = delete;
CommandInterface( CommandInterface && other ) = default;
using ReadWriteHandle::ReadWriteHandle;
};
} // namespace hardware_interface
#endif // HARDWARE_INTERFACE__HANDLE_HPP_
1 // Copyright ( c ) 2021, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Author: Denis Stogl
//
#ifndef HARDWARE_INTERFACE__HARDWARE_COMPONENT_INFO_HPP_
#define HARDWARE_INTERFACE__HARDWARE_COMPONENT_INFO_HPP_
#include <memory>
#include <string>
#include <vector>
#include "rclcpp_lifecycle/state.hpp"
namespace hardware_interface
{
/// Hardware Component Information
/**
* This struct contains information about a given hardware component.
*/
struct HardwareComponentInfo
{
/// Component name.
std::string name;
/// Component "classification": "actuator", "sensor" or "system"
std::string type;
/// Component class type.
std::string class_type;
/// Component current state.
rclcpp_lifecycle::State state;
/// List of provided state interfaces by the component.
std::vector<std::string> state_interfaces;
/// List of provided command interfaces by the component.
std::vector<std::string> command_interfaces;
};
} // namespace hardware_interface
#endif // HARDWARE_INTERFACE__HARDWARE_COMPONENT_INFO_HPP_
1 // Copyright 2020 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef HARDWARE_INTERFACE__HARDWARE_INFO_HPP_
#define HARDWARE_INTERFACE__HARDWARE_INFO_HPP_
#include <string>
#include <unordered_map>
#include <vector>
namespace hardware_interface
{
/**
* This structure stores information about components defined for a specific hardware
* in robot's URDF.
*/
struct InterfaceInfo
{
/**
* Name of the command interfaces that can be set, e.g. "position", "velocity", etc.
* Used by joints and GPIOs.
*/
std::string name;
/// ( Optional ) Minimal allowed values of the interface.
std::string min;
/// ( Optional ) Maximal allowed values of the interface.
std::string max;
/// ( Optional ) Initial value of the interface.
std::string initial_value;
/// ( Optional ) The datatype of the interface, e.g. "bool", "int". Used by GPIOs.
std::string data_type;
/// ( Optional ) If the handle is an array, the size of the array. Used by GPIOs.
int size;
};
/**
* This structure stores information about components defined for a specific hardware
* in robot's URDF.
*/
struct ComponentInfo
{
/// Name of the component.
std::string name;
/// Type of the component: sensor, joint, or GPIO.
std::string type;
/**
* Name of the command interfaces that can be set, e.g. "position", "velocity", etc.
* Used by joints and GPIOs.
*/
std::vector<InterfaceInfo> command_interfaces;
/**
* Name of the state interfaces that can be read, e.g. "position", "velocity", etc.
* Used by joints, sensors and GPIOs.
*/
std::vector<InterfaceInfo> state_interfaces;
/// ( Optional ) Key-value pairs of component parameters, e.g. min/max values or serial number.
std::unordered_map<std::string, std::string> parameters;
};
/// Contains semantic info about a given joint loaded from URDF for a transmission
struct JointInfo
{
std::string name;
std::vector<std::string> interfaces;
std::string role;
double mechanical_reduction = 1.0;
double offset = 0.0;
};
/// Contains semantic info about a given actuator loaded from URDF for a transmission
struct ActuatorInfo
{
std::string name;
std::vector<std::string> interfaces;
std::string role;
double mechanical_reduction = 1.0;
double offset = 0.0;
};
/// Contains semantic info about a given transmission loaded from URDF
struct TransmissionInfo
{
std::string name;
std::string type;
std::vector<JointInfo> joints;
std::vector<ActuatorInfo> actuators;
/// ( Optional ) Key-value pairs of custom parameters
std::unordered_map<std::string, std::string> parameters;
};
/// This structure stores information about hardware defined in a robot's URDF.
struct HardwareInfo
{
/// Name of the hardware.
std::string name;
/// Type of the hardware: actuator, sensor or system.
std::string type;
/// Class of the hardware that will be dynamically loaded.
std::string hardware_class_type;
/// ( Optional ) Key-value pairs for hardware parameters.
std::unordered_map<std::string, std::string> hardware_parameters;
/**
* Map of joints provided by the hardware where the key is the joint name.
* Required for Actuator and System Hardware.
*/
std::vector<ComponentInfo> joints;
/**
* Map of sensors provided by the hardware where the key is the joint or link name.
* Required for Sensor and optional for System Hardware.
*/
std::vector<ComponentInfo> sensors;
/**
* Map of GPIO provided by the hardware where the key is a descriptive name of the GPIO.
* Optional for any hardware components.
*/
std::vector<ComponentInfo> gpios;
/**
* Map of transmissions to calculate ration between joints and physical actuators.
* Optional for Actuator and System Hardware.
*/
std::vector<TransmissionInfo> transmissions;
/**
* The XML contents prior to parsing
*/
std::string original_xml;
};
} // namespace hardware_interface
#endif // HARDWARE_INTERFACE__HARDWARE_INFO_HPP_
// Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef HARDWARE_INTERFACE__LOANED_COMMAND_INTERFACE_HPP_
#define HARDWARE_INTERFACE__LOANED_COMMAND_INTERFACE_HPP_
#include <functional>
#include <string>
#include <utility>
#include "hardware_interface/handle.hpp"
namespace hardware_interface
{
26 class LoanedCommandInterface
{
public:
using Deleter = std::function<void( void )>;
explicit LoanedCommandInterface( CommandInterface & command_interface )
: LoanedCommandInterface( command_interface, nullptr )
{
}
LoanedCommandInterface( CommandInterface & command_interface, Deleter && deleter )
: command_interface_( command_interface ), deleter_( std::forward<Deleter>( deleter ) )
{
}
41 LoanedCommandInterface( const LoanedCommandInterface & other ) = delete;
43 LoanedCommandInterface( LoanedCommandInterface && other ) = default;
virtual ~LoanedCommandInterface( )
{
if ( deleter_ )
{
deleter_( );
}
}
const std::string get_name( ) const { return command_interface_.get_name( ); }
const std::string & get_interface_name( ) const { return command_interface_.get_interface_name( ); }
[[deprecated(
"Replaced by get_name method, which is semantically more correct" )]] const std::string
get_full_name( ) const
{
return command_interface_.get_name( );
}
const std::string & get_prefix_name( ) const { return command_interface_.get_prefix_name( ); }
void set_value( double val ) { command_interface_.set_value( val ); }
double get_value( ) const { return command_interface_.get_value( ); }
protected:
CommandInterface & command_interface_;
Deleter deleter_;
};
} // namespace hardware_interface
#endif // HARDWARE_INTERFACE__LOANED_COMMAND_INTERFACE_HPP_
// Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef HARDWARE_INTERFACE__LOANED_STATE_INTERFACE_HPP_
#define HARDWARE_INTERFACE__LOANED_STATE_INTERFACE_HPP_
#include <functional>
#include <string>
#include <utility>
#include "hardware_interface/handle.hpp"
namespace hardware_interface
{
26 class LoanedStateInterface
{
public:
using Deleter = std::function<void( void )>;
explicit LoanedStateInterface( StateInterface & state_interface )
: LoanedStateInterface( state_interface, nullptr )
{
}
LoanedStateInterface( StateInterface & state_interface, Deleter && deleter )
: state_interface_( state_interface ), deleter_( std::forward<Deleter>( deleter ) )
{
}
41 LoanedStateInterface( const LoanedStateInterface & other ) = delete;
43 LoanedStateInterface( LoanedStateInterface && other ) = default;
virtual ~LoanedStateInterface( )
{
if ( deleter_ )
{
deleter_( );
}
}
const std::string get_name( ) const { return state_interface_.get_name( ); }
const std::string & get_interface_name( ) const { return state_interface_.get_interface_name( ); }
[[deprecated(
"Replaced by get_name method, which is semantically more correct" )]] const std::string
get_full_name( ) const
{
return state_interface_.get_name( );
}
const std::string & get_prefix_name( ) const { return state_interface_.get_prefix_name( ); }
double get_value( ) const { return state_interface_.get_value( ); }
protected:
StateInterface & state_interface_;
Deleter deleter_;
};
} // namespace hardware_interface
#endif // HARDWARE_INTERFACE__LOANED_STATE_INTERFACE_HPP_
1 // Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef HARDWARE_INTERFACE__MACROS_HPP_
#define HARDWARE_INTERFACE__MACROS_HPP_
#include <stdexcept>
#include <string>
#include "rcpputils/pointer_traits.hpp"
#ifdef _WIN32
#define __PRETTY_FUNCTION__ __FUNCTION__
#endif
#define THROW_ON_NULLPTR( pointer ) \
static_assert( \
rcpputils::is_pointer<typename std::remove_reference<decltype( pointer )>::type>::value, \
#pointer " has to be a pointer" ); \
if ( !pointer ) \
{ \
throw std::runtime_error( std::string( __PRETTY_FUNCTION__ ) + " failed. " #pointer " is null." ); \
}
#define THROW_ON_NOT_NULLPTR( pointer ) \
static_assert( \
rcpputils::is_pointer<typename std::remove_reference<decltype( pointer )>::type>::value, \
#pointer " has to be a pointer" ); \
if ( pointer ) \
{ \
throw std::runtime_error( \
std::string( __PRETTY_FUNCTION__ ) + " failed. " #pointer " would leak memory" ); \
}
#endif // HARDWARE_INTERFACE__MACROS_HPP_
// Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef HARDWARE_INTERFACE__RESOURCE_MANAGER_HPP_
#define HARDWARE_INTERFACE__RESOURCE_MANAGER_HPP_
#include <memory>
#include <mutex>
#include <string>
#include <unordered_map>
#include <vector>
#include "hardware_interface/hardware_component_info.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/loaned_command_interface.hpp"
#include "hardware_interface/loaned_state_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "rclcpp/duration.hpp"
#include "rclcpp/time.hpp"
namespace hardware_interface
{
34 class ActuatorInterface;
35 class SensorInterface;
36 class SystemInterface;
37 class ResourceStorage;
class HARDWARE_INTERFACE_PUBLIC ResourceManager
{
public:
/// Default constructor for the Resource Manager.
43 ResourceManager( );
/// Constructor for the Resource Manager.
/**
* The implementation loads the specified urdf and initializes the
* hardware components listed within as well as populate their respective
* state and command interfaces.
*
* If the interfaces ought to be validated, the constructor throws an exception
* in case the URDF lists interfaces which are not available.
*
* \param[in] urdf string containing the URDF.
* \param[in] validate_interfaces boolean argument indicating whether the exported
* interfaces ought to be validated. Defaults to true.
* \param[in] activate_all boolean argument indicating if all resources should be immediately
* activated. Currently used only in tests. In typical applications use parameters
* "autostart_components" and "autoconfigure_components" instead.
*/
61 explicit ResourceManager(
const std::string & urdf, bool validate_interfaces = true, bool activate_all = false );
64 ResourceManager( const ResourceManager & ) = delete;
66 ~ResourceManager( );
/// Load resources from on a given URDF.
/**
* The resource manager can be post initialized with a given URDF.
* This is mainly used in conjunction with the default constructor
* in which the URDF might not be present at first initialization.
*
* \param[in] urdf string containing the URDF.
* \param[in] validate_interfaces boolean argument indicating whether the exported
* interfaces ought to be validated. Defaults to true.
*/
78 void load_urdf( const std::string & urdf, bool validate_interfaces = true );
/// Claim a state interface given its key.
/**
* The resource is claimed as long as being in scope.
* Once the resource is going out of scope, the destructor
* returns.
*
* \param[in] key String identifier which state interface to claim
* \return state interface
*/
89 LoanedStateInterface claim_state_interface( const std::string & key );
/// Returns all registered state interfaces keys.
/**
* The keys are collected from each loaded hardware component.
* \return Vector of strings, containing all registered keys.
*/
96 std::vector<std::string> state_interface_keys( ) const;
/// Returns all available state interfaces keys.
/**
* The keys are collected from the available list.
* \return Vector of strings, containing all available state interface names.
*/
103 std::vector<std::string> available_state_interfaces( ) const;
/// Checks whether a state interface is registered under the given key.
/**
* \return true if interface exist, false otherwise.
*/
109 bool state_interface_exists( const std::string & key ) const;
/// Checks whether a state interface is available under the given key.
/**
* \return true if interface is available, false otherwise.
*/
115 bool state_interface_is_available( const std::string & name ) const;
/// Add controllers' reference interfaces to resource manager.
/**
* Interface for transferring management of reference interfaces to resource manager.
* When chaining controllers, reference interfaces are used as command interface of preceding
* controllers.
* Therefore, they should be managed in the same way as command interface of hardware.
*
* \param[in] controller_name name of the controller which reference interfaces are imported.
* \param[in] interfaces list of controller's reference interfaces as CommandInterfaces.
*/
127 void import_controller_reference_interfaces(
const std::string & controller_name, std::vector<CommandInterface> & interfaces );
/// Get list of reference interface of a controller.
/**
* Returns lists of stored reference interfaces names for a controller.
*
* \param[in] controller_name for which list of reference interface names is returned.
* \returns list of reference interface names.
*/
137 std::vector<std::string> get_controller_reference_interface_names(
const std::string & controller_name );
/// Add controller's reference interface to available list.
/**
* Adds interfaces of a controller with given name to the available list. This method should be
* called when a controller gets activated with chained mode turned on. That means, the
* controller's reference interfaces can be used by another controller in chained architectures.
*
* \param[in] controller_name name of the controller which interfaces should become available.
*/
148 void make_controller_reference_interfaces_available( const std::string & controller_name );
/// Remove controller's reference interface to available list.
/**
* Removes interfaces of a controller with given name from the available list. This method should
* be called when a controller gets deactivated and its reference interfaces cannot be used by
* another controller anymore.
*
* \param[in] controller_name name of the controller which interfaces should become unavailable.
*/
158 void make_controller_reference_interfaces_unavailable( const std::string & controller_name );
/// Remove controllers reference interfaces from resource manager.
/**
* Remove reference interfaces from resource manager, i.e., resource storage.
* The interfaces will be deleted from all internal maps and lists.
*
* \param[in] controller_name list of interface names that will be deleted from resource manager.
*/
167 void remove_controller_reference_interfaces( const std::string & controller_name );
/// Checks whether a command interface is already claimed.
/**
* Any command interface can only be claimed by a single instance.
* \note the equivalent function does not exist for state interfaces.
* These are solely read-only and can thus be used by multiple instances.
*
* \param[in] key string identifying the interface to check.
* \return true if interface is already claimed, false if available.
*/
178 bool command_interface_is_claimed( const std::string & key ) const;
/// Claim a command interface given its key.
/**
* The resource is claimed as long as being in scope.
* Once the resource is going out of scope, the destructor
* returns and thus frees the resource to claimed by others.
*
* \param[in] key String identifier which command interface to claim
* \return command interface
*/
189 LoanedCommandInterface claim_command_interface( const std::string & key );
/// Returns all registered command interfaces keys.
/**
* The keys are collected from each loaded hardware component.
* \return vector of strings, containing all registered keys.
*/
196 std::vector<std::string> command_interface_keys( ) const;
/// Returns all available command interfaces keys.
/**
* The keys are collected from the available list.
* \return vector of strings, containing all available command interface names.
*/
203 std::vector<std::string> available_command_interfaces( ) const;
/// Checks whether a command interface is registered under the given key.
/**
* \param[in] key string identifying the interface to check.
* \return true if interface exist, false otherwise.
*/
210 bool command_interface_exists( const std::string & key ) const;
/// Checks whether a command interface is available under the given name.
/**
* \param[in] name string identifying the interface to check.
* \return true if interface is available, false otherwise.
*/
217 bool command_interface_is_available( const std::string & interface ) const;
/// Return the number size_t of loaded actuator components.
/**
* \return number of actuator components.
*/
223 size_t actuator_components_size( ) const;
/// Return the number of loaded sensor components.
/**
* \return number of sensor components.
*/
229 size_t sensor_components_size( ) const;
/// Return the number of loaded system components.
/**
* \return size_t number of system components.
*/
235 size_t system_components_size( ) const;
/// Import a hardware component which is not listed in the URDF
/**
* Components which are initialized outside a URDF can be added post initialization.
* Nevertheless, there should still be `HardwareInfo` available for this component,
* either parsed from a URDF string ( easiest ) or filled manually.
*
* \note this might invalidate existing state and command interfaces and should thus
* not be called when a controller is running.
* \note given that no hardware_info is available, the component has to be configured
* externally and prior to the call to import.
* \param[in] actuator pointer to the actuator interface.
* \param[in] hardware_info hardware info
*/
250 void import_component(
std::unique_ptr<ActuatorInterface> actuator, const HardwareInfo & hardware_info );
/// Import a hardware component which is not listed in the URDF
/**
* Components which are initialized outside a URDF can be added post initialization.
* Nevertheless, there should still be `HardwareInfo` available for this component,
* either parsed from a URDF string ( easiest ) or filled manually.
*
* \note this might invalidate existing state and command interfaces and should thus
* not be called when a controller is running.
* \note given that no hardware_info is available, the component has to be configured
* externally and prior to the call to import.
* \param[in] sensor pointer to the sensor interface.
* \param[in] hardware_info hardware info
*/
266 void import_component(
std::unique_ptr<SensorInterface> sensor, const HardwareInfo & hardware_info );
/// Import a hardware component which is not listed in the URDF
/**
* Components which are initialized outside a URDF can be added post initialization.
* Nevertheless, there should still be `HardwareInfo` available for this component,
* either parsed from a URDF string ( easiest ) or filled manually.
*
* \note this might invalidate existing state and command interfaces and should thus
* not be called when a controller is running.
* \note given that no hardware_info is available, the component has to be configured
* externally and prior to the call to import.
* \param[in] system pointer to the system interface.
* \param[in] hardware_info hardware info
*/
282 void import_component(
std::unique_ptr<SystemInterface> system, const HardwareInfo & hardware_info );
/// Return status for all components.
/**
* \return map of hardware names and their status.
*/
289 std::unordered_map<std::string, HardwareComponentInfo> get_components_status( );
/// Prepare the hardware components for a new command interface mode
/**
* Hardware components are asked to prepare a new command interface claim.
*
* \note this is intended for mode-switching when a hardware interface needs to change
* control mode depending on which command interface is claimed.
* \note this is for non-realtime preparing for and accepting new command resource
* combinations.
* \note accept_command_resource_claim is called on all actuators and system components
* and hardware interfaces should return hardware_interface::return_type::OK
* by default
* \param[in] start_interfaces vector of string identifiers for the command interfaces starting.
* \param[in] stop_interfaces vector of string identifiers for the command interfaces stopping.
* \return true if switch can be prepared, false if a component rejects switch request.
*/
306 bool prepare_command_mode_switch(
const std::vector<std::string> & start_interfaces,
const std::vector<std::string> & stop_interfaces );
/// Notify the hardware components that realtime hardware mode switching should occur.
/**
* Hardware components are asked to perform the command interface mode switching.
*
* \note this is intended for mode-switching when a hardware interface needs to change
* control mode depending on which command interface is claimed.
* \note this is for realtime switching of the command interface.
* \param[in] start_interfaces vector of string identifiers for the command interfaces starting.
* \param[in] stop_interfaces vector of string identifiers for the command interfacs stopping.
* \return true if switch is performed, false if a component rejects switching.
*/
321 bool perform_command_mode_switch(
const std::vector<std::string> & start_interfaces,
const std::vector<std::string> & stop_interfaces );
/// Sets state of hardware component.
/**
* Set set of hardware component if possible.
* Takes care of all transitions needed to reach the target state.
* It implements the state machine from: https://design.ros2.org/articles/node_lifecycle.html
*
* The method is not part of the real-time critical update loop.
*
* \param[in] component_name component name to change state.
* \param[in] target_state target state to set for a hardware component.
* \return hardware_interface::retun_type::OK if component successfully switched its state and
* hardware_interface::return_type::ERROR any of state transitions has failed.
*/
338 return_type set_component_state(
const std::string & component_name, rclcpp_lifecycle::State & target_state );
/// Reads all loaded hardware components.
/**
* Reads from all active hardware components.
*
* Part of the real-time critical update loop.
* It is realtime-safe if used hadware interfaces are implemented adequately.
*/
348 void read( const rclcpp::Time & time, const rclcpp::Duration & period );
/// Write all loaded hardware components.
/**
* Writes to all active hardware components.
*
* Part of the real-time critical update loop.
* It is realtime-safe if used hadware interfaces are implemented adequately.
*/
357 void write( const rclcpp::Time & time, const rclcpp::Duration & period );
/// Activates all available hardware components in the system.
/**
* All available hardware components int the ros2_control framework are activated.
* This is used to preserve default behavior from previous versions where all hardware components
* are activated per default.
*/
365 void activate_all_components( );
private:
void validate_storage( const std::vector<hardware_interface::HardwareInfo> & hardware_info ) const;
void release_command_interface( const std::string & key );
std::unordered_map<std::string, bool> claimed_command_interface_map_;
mutable std::recursive_mutex resource_interfaces_lock_;
mutable std::recursive_mutex claimed_command_interfaces_lock_;
std::unique_ptr<ResourceStorage> resource_storage_;
};
} // namespace hardware_interface
#endif // HARDWARE_INTERFACE__RESOURCE_MANAGER_HPP_
// Copyright 2020 - 2021 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef HARDWARE_INTERFACE__SENSOR_HPP_
#define HARDWARE_INTERFACE__SENSOR_HPP_
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/visibility_control.h"
#include "rclcpp/duration.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp_lifecycle/state.hpp"
namespace hardware_interface
{
33 class SensorInterface;
class Sensor final
{
public:
38 Sensor( ) = default;
HARDWARE_INTERFACE_PUBLIC
explicit Sensor( std::unique_ptr<SensorInterface> impl );
43 Sensor( Sensor && other ) = default;
45 ~Sensor( ) = default;
47 HARDWARE_INTERFACE_PUBLIC
const rclcpp_lifecycle::State & initialize( const HardwareInfo & sensor_info );
50 HARDWARE_INTERFACE_PUBLIC
const rclcpp_lifecycle::State & configure( );
HARDWARE_INTERFACE_PUBLIC
54 const rclcpp_lifecycle::State & cleanup( );
56 HARDWARE_INTERFACE_PUBLIC
const rclcpp_lifecycle::State & shutdown( );
59 HARDWARE_INTERFACE_PUBLIC
const rclcpp_lifecycle::State & activate( );
HARDWARE_INTERFACE_PUBLIC
63 const rclcpp_lifecycle::State & deactivate( );
65 HARDWARE_INTERFACE_PUBLIC
const rclcpp_lifecycle::State & error( );
HARDWARE_INTERFACE_PUBLIC
std::vector<StateInterface> export_state_interfaces( );
HARDWARE_INTERFACE_PUBLIC
72 std::string get_name( ) const;
HARDWARE_INTERFACE_PUBLIC
75 const rclcpp_lifecycle::State & get_state( ) const;
HARDWARE_INTERFACE_PUBLIC
return_type read( const rclcpp::Time & time, const rclcpp::Duration & period );
private:
std::unique_ptr<SensorInterface> impl_;
};
} // namespace hardware_interface
#endif // HARDWARE_INTERFACE__SENSOR_HPP_
// Copyright 2020 - 2021 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_
#define HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_
#include <memory>
#include <string>
#include <vector>
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/duration.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
namespace hardware_interface
{
/// Virtual Class to implement when integrating a stand-alone sensor into ros2_control.
/**
* The typical examples are Force-Torque Sensor ( FTS ), Interial Measurement Unit ( IMU ).
*
* Methods return values have type rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
* with the following meaning:
*
* \returns CallbackReturn::SUCCESS method execution was successful.
* \returns CallbackReturn::FAILURE method execution has failed and and can be called again.
* \returns CallbackReturn::ERROR critical error has happened that should be managed in
* "on_error" method.
*
* The hardware ends after each method in a state with the following meaning:
*
* UNCONFIGURED ( on_init, on_cleanup ):
* Hardware is initialized but communication is not started and therefore no interface is available.
*
* INACTIVE ( on_configure, on_deactivate ):
* Communication with the hardware is started and it is configured.
* States can be read and non-movement hardware interfaces commanded.
* Hardware interfaces for movement will NOT be available.
* Those interfaces are: HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_ACCELERATION, and HW_IF_EFFORT.
*
* FINALIZED ( on_shutdown ):
* Hardware interface is ready for unloading/destruction.
* Allocated memory is cleaned up.
*
* ACTIVE ( on_activate ):
* Power circuits of hardware are active and hardware can be moved, e.g., brakes are disabled.
* Command interfaces for movement are available and have to be accepted.
* Those interfaces are: HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_ACCELERATION, and HW_IF_EFFORT.
*/
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
69 class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
{
public:
72 SensorInterface( )
: lifecycle_state_( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_state_names::UNKNOWN ) )
{
}
/// SensorInterface copy constructor is actively deleted.
/**
* Hardware interfaces are having a unique ownership and thus can't be copied in order to avoid
* failed or simultaneous access to hardware.
*/
SensorInterface( const SensorInterface & other ) = delete;
SensorInterface( SensorInterface && other ) = default;
virtual ~SensorInterface( ) = default;
/// Initialization of the hardware interface from data parsed from the robot's URDF.
/**
* \param[in] hardware_info structure with data from URDF.
* \returns CallbackReturn::SUCCESS if required data are provided and can be parsed.
* \returns CallbackReturn::ERROR if any error happens or data are missing.
*/
virtual CallbackReturn on_init( const HardwareInfo & hardware_info )
{
info_ = hardware_info;
return CallbackReturn::SUCCESS;
};
/// Exports all state interfaces for this hardware interface.
/**
* The state interfaces have to be created and transferred according
* to the hardware info passed in for the configuration.
*
* Note the ownership over the state interfaces is transferred to the caller.
*
* \return vector of state interfaces
*/
110 virtual std::vector<StateInterface> export_state_interfaces( ) = 0;
/// Read the current state values from the actuator.
/**
* The data readings from the physical hardware has to be updated
* and reflected accordingly in the exported state interfaces.
* That is, the data pointed by the interfaces shall be updated.
*
* \return return_type::OK if the read was successful, return_type::ERROR otherwise.
*/
virtual return_type read( const rclcpp::Time & time, const rclcpp::Duration & period ) = 0;
/// Get name of the actuator hardware.
/**
* \return name.
*/
virtual std::string get_name( ) const { return info_.name; }
/// Get life-cycle state of the actuator hardware.
/**
* \return state.
*/
const rclcpp_lifecycle::State & get_state( ) const { return lifecycle_state_; }
/// Set life-cycle state of the actuator hardware.
/**
* \return state.
*/
void set_state( const rclcpp_lifecycle::State & new_state ) { lifecycle_state_ = new_state; }
protected:
HardwareInfo info_;
rclcpp_lifecycle::State lifecycle_state_;
};
} // namespace hardware_interface
#endif // HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_
// Copyright 2020 - 2021 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef HARDWARE_INTERFACE__SYSTEM_HPP_
#define HARDWARE_INTERFACE__SYSTEM_HPP_
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/visibility_control.h"
#include "rclcpp/duration.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp_lifecycle/state.hpp"
namespace hardware_interface
{
33 class SystemInterface;
class System final
{
public:
HARDWARE_INTERFACE_PUBLIC
explicit System( std::unique_ptr<SystemInterface> impl );
41 System( System && other ) = default;
43 ~System( ) = default;
45 HARDWARE_INTERFACE_PUBLIC
const rclcpp_lifecycle::State & initialize( const HardwareInfo & system_info );
48 HARDWARE_INTERFACE_PUBLIC
const rclcpp_lifecycle::State & configure( );
HARDWARE_INTERFACE_PUBLIC
52 const rclcpp_lifecycle::State & cleanup( );
54 HARDWARE_INTERFACE_PUBLIC
const rclcpp_lifecycle::State & shutdown( );
57 HARDWARE_INTERFACE_PUBLIC
const rclcpp_lifecycle::State & activate( );
HARDWARE_INTERFACE_PUBLIC
61 const rclcpp_lifecycle::State & deactivate( );
63 HARDWARE_INTERFACE_PUBLIC
const rclcpp_lifecycle::State & error( );
HARDWARE_INTERFACE_PUBLIC
std::vector<StateInterface> export_state_interfaces( );
HARDWARE_INTERFACE_PUBLIC
std::vector<CommandInterface> export_command_interfaces( );
HARDWARE_INTERFACE_PUBLIC
return_type prepare_command_mode_switch(
const std::vector<std::string> & start_interfaces,
const std::vector<std::string> & stop_interfaces );
HARDWARE_INTERFACE_PUBLIC
return_type perform_command_mode_switch(
const std::vector<std::string> & start_interfaces,
const std::vector<std::string> & stop_interfaces );
HARDWARE_INTERFACE_PUBLIC
83 std::string get_name( ) const;
HARDWARE_INTERFACE_PUBLIC
86 const rclcpp_lifecycle::State & get_state( ) const;
HARDWARE_INTERFACE_PUBLIC
return_type read( const rclcpp::Time & time, const rclcpp::Duration & period );
HARDWARE_INTERFACE_PUBLIC
return_type write( const rclcpp::Time & time, const rclcpp::Duration & period );
private:
std::unique_ptr<SystemInterface> impl_;
};
} // namespace hardware_interface
#endif // HARDWARE_INTERFACE__SYSTEM_HPP_
// Copyright 2020 - 2021 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_
#define HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_
#include <memory>
#include <string>
#include <vector>
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/duration.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
namespace hardware_interface
{
/// Virtual Class to implement when integrating a complex system into ros2_control.
/**
* The common examples for these types of hardware are multi-joint systems with or without sensors
* such as industrial or humanoid robots.
*
* Methods return values have type rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
* with the following meaning:
*
* \returns CallbackReturn::SUCCESS method execution was successful.
* \returns CallbackReturn::FAILURE method execution has failed and and can be called again.
* \returns CallbackReturn::ERROR critical error has happened that should be managed in
* "on_error" method.
*
* The hardware ends after each method in a state with the following meaning:
*
* UNCONFIGURED ( on_init, on_cleanup ):
* Hardware is initialized but communication is not started and therefore no interface is available.
*
* INACTIVE ( on_configure, on_deactivate ):
* Communication with the hardware is started and it is configured.
* States can be read and non-movement hardware interfaces commanded.
* Hardware interfaces for movement will NOT be available.
* Those interfaces are: HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_ACCELERATION, and HW_IF_EFFORT.
*
* FINALIZED ( on_shutdown ):
* Hardware interface is ready for unloading/destruction.
* Allocated memory is cleaned up.
*
* ACTIVE ( on_activate ):
* Power circuits of hardware are active and hardware can be moved, e.g., brakes are disabled.
* Command interfaces for movement are available and have to be accepted.
* Those interfaces are: HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_ACCELERATION, and HW_IF_EFFORT.
*/
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
70 class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
{
public:
73 SystemInterface( )
: lifecycle_state_( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_state_names::UNKNOWN ) )
{
}
/// SystemInterface copy constructor is actively deleted.
/**
* Hardware interfaces are having a unique ownership and thus can't be copied in order to avoid
* failed or simultaneous access to hardware.
*/
SystemInterface( const SystemInterface & other ) = delete;
SystemInterface( SystemInterface && other ) = default;
virtual ~SystemInterface( ) = default;
/// Initialization of the hardware interface from data parsed from the robot's URDF.
/**
* \param[in] hardware_info structure with data from URDF.
* \returns CallbackReturn::SUCCESS if required data are provided and can be parsed.
* \returns CallbackReturn::ERROR if any error happens or data are missing.
*/
virtual CallbackReturn on_init( const HardwareInfo & hardware_info )
{
info_ = hardware_info;
return CallbackReturn::SUCCESS;
};
/// Exports all state interfaces for this hardware interface.
/**
* The state interfaces have to be created and transferred according
* to the hardware info passed in for the configuration.
*
* Note the ownership over the state interfaces is transferred to the caller.
*
* \return vector of state interfaces
*/
111 virtual std::vector<StateInterface> export_state_interfaces( ) = 0;
/// Exports all command interfaces for this hardware interface.
/**
* The command interfaces have to be created and transferred according
* to the hardware info passed in for the configuration.
*
* Note the ownership over the state interfaces is transferred to the caller.
*
* \return vector of command interfaces
*/
virtual std::vector<CommandInterface> export_command_interfaces( ) = 0;
/// Prepare for a new command interface switch.
/**
* Prepare for any mode-switching required by the new command interface combination.
*
* \note This is a non-realtime evaluation of whether a set of command interface claims are
* possible, and call to start preparing data structures for the upcoming switch that will occur.
* \note All starting and stopping interface keys are passed to all components, so the function should
* return return_type::OK by default when given interface keys not relevant for this component.
* \param[in] start_interfaces vector of string identifiers for the command interfaces starting.
* \param[in] stop_interfaces vector of string identifiers for the command interfacs stopping.
* \return return_type::OK if the new command interface combination can be prepared,
* or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise.
*/
virtual return_type prepare_command_mode_switch(
const std::vector<std::string> & /*start_interfaces*/,
const std::vector<std::string> & /*stop_interfaces*/ )
{
return return_type::OK;
}
// Perform switching to the new command interface.
/**
* Perform the mode-switching for the new command interface combination.
*
* \note This is part of the realtime update loop, and should be fast.
* \note All starting and stopping interface keys are passed to all components, so the function should
* return return_type::OK by default when given interface keys not relevant for this component.
* \param[in] start_interfaces vector of string identifiers for the command interfaces starting.
* \param[in] stop_interfaces vector of string identifiers for the command interfacs stopping.
* \return return_type::OK if the new command interface combination can be switched to,
* or if the interface key is not relevant to this system. Returns return_type::ERROR otherwise.
*/
virtual return_type perform_command_mode_switch(
const std::vector<std::string> & /*start_interfaces*/,
const std::vector<std::string> & /*stop_interfaces*/ )
{
return return_type::OK;
}
/// Read the current state values from the actuator.
/**
* The data readings from the physical hardware has to be updated
* and reflected accordingly in the exported state interfaces.
* That is, the data pointed by the interfaces shall be updated.
*
* \return return_type::OK if the read was successful, return_type::ERROR otherwise.
*/
virtual return_type read( const rclcpp::Time & time, const rclcpp::Duration & period ) = 0;
/// Write the current command values to the actuator.
/**
* The physical hardware shall be updated with the latest value from
* the exported command interfaces.
*
* \return return_type::OK if the read was successful, return_type::ERROR otherwise.
*/
virtual return_type write( const rclcpp::Time & time, const rclcpp::Duration & period ) = 0;
/// Get name of the actuator hardware.
/**
* \return name.
*/
virtual std::string get_name( ) const { return info_.name; }
/// Get life-cycle state of the actuator hardware.
/**
* \return state.
*/
const rclcpp_lifecycle::State & get_state( ) const { return lifecycle_state_; }
/// Set life-cycle state of the actuator hardware.
/**
* \return state.
*/
void set_state( const rclcpp_lifecycle::State & new_state ) { lifecycle_state_ = new_state; }
protected:
HardwareInfo info_;
rclcpp_lifecycle::State lifecycle_state_;
};
} // namespace hardware_interface
#endif // HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_
1 // Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_RETURN_VALUES_HPP_
#define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_RETURN_VALUES_HPP_
#include <cstdint>
namespace hardware_interface
{
22 enum class return_type : std::uint8_t
{
OK = 0,
ERROR = 1,
};
} // namespace hardware_interface
#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_RETURN_VALUES_HPP_
1 // Copyright 2020 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_
#define HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_
namespace hardware_interface
{
/// Constant defining position interface
21 constexpr char HW_IF_POSITION[] = "position";
/// Constant defining velocity interface
23 constexpr char HW_IF_VELOCITY[] = "velocity";
/// Constant defining acceleration interface
constexpr char HW_IF_ACCELERATION[] = "acceleration";
/// Constant defining effort interface
27 constexpr char HW_IF_EFFORT[] = "effort";
} // namespace hardware_interface
#endif // HARDWARE_INTERFACE__TYPES__HARDWARE_INTERFACE_TYPE_VALUES_HPP_
1 // Copyright ( c ) 2021, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
/// \author: Denis Stogl
#ifndef HARDWARE_INTERFACE__TYPES__LIFECYCLE_STATE_NAMES_HPP_
#define HARDWARE_INTERFACE__TYPES__LIFECYCLE_STATE_NAMES_HPP_
#include <string>
namespace hardware_interface
{
namespace lifecycle_state_names
{
/// Constants defining string labels corresponding to lifecycle states
27 constexpr char UNKNOWN[] = "unknown";
28 constexpr char UNCONFIGURED[] = "unconfigured";
constexpr char INACTIVE[] = "inactive";
30 constexpr char ACTIVE[] = "active";
31 constexpr char FINALIZED[] = "finalized";
} // namespace lifecycle_state_names
} // namespace hardware_interface
#endif // HARDWARE_INTERFACE__TYPES__LIFECYCLE_STATE_NAMES_HPP_
1 // Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef HARDWARE_INTERFACE__VISIBILITY_CONTROL_H_
#define HARDWARE_INTERFACE__VISIBILITY_CONTROL_H_
// This logic was borrowed ( then namespaced ) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define HARDWARE_INTERFACE_EXPORT __attribute__( ( dllexport ) )
#define HARDWARE_INTERFACE_IMPORT __attribute__( ( dllimport ) )
#else
#define HARDWARE_INTERFACE_EXPORT __declspec( dllexport )
#define HARDWARE_INTERFACE_IMPORT __declspec( dllimport )
#endif
#ifdef HARDWARE_INTERFACE_BUILDING_DLL
#define HARDWARE_INTERFACE_PUBLIC HARDWARE_INTERFACE_EXPORT
#else
#define HARDWARE_INTERFACE_PUBLIC HARDWARE_INTERFACE_IMPORT
#endif
#define HARDWARE_INTERFACE_PUBLIC_TYPE HARDWARE_INTERFACE_PUBLIC
#define HARDWARE_INTERFACE_LOCAL
#else
#define HARDWARE_INTERFACE_EXPORT __attribute__( ( visibility( "default" ) ) )
#define HARDWARE_INTERFACE_IMPORT
#if __GNUC__ >= 4
#define HARDWARE_INTERFACE_PUBLIC __attribute__( ( visibility( "default" ) ) )
#define HARDWARE_INTERFACE_LOCAL __attribute__( ( visibility( "hidden" ) ) )
#else
#define HARDWARE_INTERFACE_PUBLIC
#define HARDWARE_INTERFACE_LOCAL
#endif
#define HARDWARE_INTERFACE_PUBLIC_TYPE
#endif
#endif // HARDWARE_INTERFACE__VISIBILITY_CONTROL_H_
// Copyright ( c ) 2021 PickNik, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Author: Jafar Abdi, Denis Stogl
#ifndef MOCK_COMPONENTS__GENERIC_SYSTEM_HPP_
#define MOCK_COMPONENTS__GENERIC_SYSTEM_HPP_
#include <string>
#include <vector>
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
using hardware_interface::return_type;
namespace mock_components
{
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
class HARDWARE_INTERFACE_PUBLIC GenericSystem : public hardware_interface::SystemInterface
{
public:
38 CallbackReturn on_init( const hardware_interface::HardwareInfo & info ) override;
40 std::vector<hardware_interface::StateInterface> export_state_interfaces( ) override;
42 std::vector<hardware_interface::CommandInterface> export_command_interfaces( ) override;
44 return_type read( const rclcpp::Time & time, const rclcpp::Duration & period ) override;
46 return_type write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) override
{
return return_type::OK;
}
protected:
/// Use standard interfaces for joints because they are relevant for dynamic behavior
/**
* By splitting the standard interfaces from other type, the users are able to inherit this
* class and simply create small "simulation" with desired dynamic behavior.
* The advantage over using Gazebo is that enables "quick & dirty" tests of robot's URDF and
* controllers.
*/
const std::vector<std::string> standard_interfaces_ = {
hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY,
hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT};
const size_t POSITION_INTERFACE_INDEX = 0;
struct MimicJoint
{
std::size_t joint_index;
std::size_t mimicked_joint_index;
double multiplier = 1.0;
};
std::vector<MimicJoint> mimic_joints_;
/// The size of this vector is ( standard_interfaces_.size( ) x nr_joints )
std::vector<std::vector<double>> joint_commands_;
std::vector<std::vector<double>> joint_states_;
std::vector<std::string> other_interfaces_;
/// The size of this vector is ( other_interfaces_.size( ) x nr_joints )
std::vector<std::vector<double>> other_commands_;
std::vector<std::vector<double>> other_states_;
std::vector<std::string> sensor_interfaces_;
/// The size of this vector is ( sensor_interfaces_.size( ) x nr_joints )
std::vector<std::vector<double>> sensor_fake_commands_;
std::vector<std::vector<double>> sensor_states_;
std::vector<std::string> gpio_interfaces_;
/// The size of this vector is ( gpio_interfaces_.size( ) x nr_joints )
std::vector<std::vector<double>> gpio_fake_commands_;
std::vector<std::vector<double>> gpio_commands_;
std::vector<std::vector<double>> gpio_states_;
private:
template <typename HandleType>
bool get_interface(
const std::string & name, const std::vector<std::string> & interface_list,
const std::string & interface_name, const size_t vector_index,
std::vector<std::vector<double>> & values, std::vector<HandleType> & interfaces );
void initialize_storage_vectors(
std::vector<std::vector<double>> & commands, std::vector<std::vector<double>> & states,
const std::vector<std::string> & interfaces );
template <typename InterfaceType>
bool populate_interfaces(
const std::vector<hardware_interface::ComponentInfo> & components,
std::vector<std::string> & interfaces, std::vector<std::vector<double>> & storage,
std::vector<InterfaceType> & target_interfaces, bool using_state_interfaces );
bool use_fake_gpio_command_interfaces_;
bool use_fake_sensor_command_interfaces_;
double position_state_following_offset_;
std::string custom_interface_with_following_offset_;
size_t index_custom_interface_with_following_offset_;
};
typedef GenericSystem GenericRobot;
} // namespace mock_components
#endif // MOCK_COMPONENTS__GENERIC_SYSTEM_HPP_
1 // Copyright ( c ) 2021 PickNik, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef MOCK_COMPONENTS__VISIBILITY_CONTROL_H_
#define MOCK_COMPONENTS__VISIBILITY_CONTROL_H_
// This logic was borrowed ( then namespaced ) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define MOCK_COMPONENTS_EXPORT __attribute__( ( dllexport ) )
#define MOCK_COMPONENTS_IMPORT __attribute__( ( dllimport ) )
#else
#define MOCK_COMPONENTS_EXPORT __declspec( dllexport )
#define MOCK_COMPONENTS_IMPORT __declspec( dllimport )
#endif
#ifdef MOCK_COMPONENTS_BUILDING_DLL
#define MOCK_COMPONENTS_PUBLIC MOCK_COMPONENTS_EXPORT
#else
#define MOCK_COMPONENTS_PUBLIC MOCK_COMPONENTS_IMPORT
#endif
#define MOCK_COMPONENTS_PUBLIC_TYPE MOCK_COMPONENTS_PUBLIC
#define MOCK_COMPONENTS_LOCAL
#else
#define MOCK_COMPONENTS_EXPORT __attribute__( ( visibility( "default" ) ) )
#define MOCK_COMPONENTS_IMPORT
#if __GNUC__ >= 4
#define MOCK_COMPONENTS_PUBLIC __attribute__( ( visibility( "default" ) ) )
#define MOCK_COMPONENTS_LOCAL __attribute__( ( visibility( "hidden" ) ) )
#else
#define MOCK_COMPONENTS_PUBLIC
#define MOCK_COMPONENTS_LOCAL
#endif
#define MOCK_COMPONENTS_PUBLIC_TYPE
#endif
#endif // MOCK_COMPONENTS__VISIBILITY_CONTROL_H_
1 // Copyright 2020-2021 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "hardware_interface/actuator.hpp"
#include <algorithm>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "hardware_interface/actuator_interface.hpp"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
// TODO( destogl ): Add transition names also
namespace hardware_interface
{
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
37 Actuator::Actuator( std::unique_ptr<ActuatorInterface> impl ) : impl_( std::move( impl ) ) {}
39 const rclcpp_lifecycle::State & Actuator::initialize( const HardwareInfo & actuator_info )
{
if ( impl_->get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN )
{
switch ( impl_->on_init( actuator_info ) )
{
case CallbackReturn::SUCCESS:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
lifecycle_state_names::UNCONFIGURED ) );
break;
case CallbackReturn::FAILURE:
case CallbackReturn::ERROR:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED ) );
break;
}
}
return impl_->get_state( );
}
60 const rclcpp_lifecycle::State & Actuator::configure( )
{
if ( impl_->get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED )
{
switch ( impl_->on_configure( impl_->get_state( ) ) )
{
case CallbackReturn::SUCCESS:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE ) );
break;
case CallbackReturn::FAILURE:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
lifecycle_state_names::UNCONFIGURED ) );
break;
case CallbackReturn::ERROR:
impl_->set_state( error( ) );
break;
}
}
return impl_->get_state( );
}
83 const rclcpp_lifecycle::State & Actuator::cleanup( )
{
if ( impl_->get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE )
{
switch ( impl_->on_cleanup( impl_->get_state( ) ) )
{
case CallbackReturn::SUCCESS:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
lifecycle_state_names::UNCONFIGURED ) );
break;
case CallbackReturn::FAILURE:
case CallbackReturn::ERROR:
impl_->set_state( error( ) );
break;
}
}
return impl_->get_state( );
}
103 const rclcpp_lifecycle::State & Actuator::shutdown( )
{
if (
impl_->get_state( ).id( ) != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN &&
impl_->get_state( ).id( ) != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED )
{
switch ( impl_->on_shutdown( impl_->get_state( ) ) )
{
case CallbackReturn::SUCCESS:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED ) );
break;
case CallbackReturn::FAILURE:
case CallbackReturn::ERROR:
impl_->set_state( error( ) );
break;
}
}
return impl_->get_state( );
}
124 const rclcpp_lifecycle::State & Actuator::activate( )
{
if ( impl_->get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE )
{
switch ( impl_->on_activate( impl_->get_state( ) ) )
{
case CallbackReturn::SUCCESS:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE ) );
break;
case CallbackReturn::FAILURE:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE ) );
break;
case CallbackReturn::ERROR:
impl_->set_state( error( ) );
break;
}
}
return impl_->get_state( );
}
146 const rclcpp_lifecycle::State & Actuator::deactivate( )
{
if ( impl_->get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
{
switch ( impl_->on_deactivate( impl_->get_state( ) ) )
{
case CallbackReturn::SUCCESS:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE ) );
break;
case CallbackReturn::FAILURE:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE ) );
break;
case CallbackReturn::ERROR:
impl_->set_state( error( ) );
break;
}
}
return impl_->get_state( );
}
168 const rclcpp_lifecycle::State & Actuator::error( )
{
if ( impl_->get_state( ).id( ) != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN )
{
switch ( impl_->on_error( impl_->get_state( ) ) )
{
case CallbackReturn::SUCCESS:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
lifecycle_state_names::UNCONFIGURED ) );
break;
case CallbackReturn::FAILURE:
case CallbackReturn::ERROR:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED ) );
break;
}
}
return impl_->get_state( );
}
189 std::vector<StateInterface> Actuator::export_state_interfaces( )
{
// TODO( karsten1987 ): Might be worth to do some brief sanity check here
return impl_->export_state_interfaces( );
}
195 std::vector<CommandInterface> Actuator::export_command_interfaces( )
{
// TODO( karsten1987 ): Might be worth to do some brief sanity check here
return impl_->export_command_interfaces( );
}
201 return_type Actuator::prepare_command_mode_switch(
202 const std::vector<std::string> & start_interfaces,
203 const std::vector<std::string> & stop_interfaces )
{
return impl_->prepare_command_mode_switch( start_interfaces, stop_interfaces );
}
208 return_type Actuator::perform_command_mode_switch(
209 const std::vector<std::string> & start_interfaces,
210 const std::vector<std::string> & stop_interfaces )
{
return impl_->perform_command_mode_switch( start_interfaces, stop_interfaces );
}
215 std::string Actuator::get_name( ) const { return impl_->get_name( ); }
217 const rclcpp_lifecycle::State & Actuator::get_state( ) const { return impl_->get_state( ); }
219 return_type Actuator::read( const rclcpp::Time & time, const rclcpp::Duration & period )
{
return_type result = return_type::ERROR;
if (
impl_->get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
impl_->get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
{
result = impl_->read( time, period );
if ( result == return_type::ERROR )
{
error( );
}
}
return result;
}
235 return_type Actuator::write( const rclcpp::Time & time, const rclcpp::Duration & period )
{
return_type result = return_type::ERROR;
if (
impl_->get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
impl_->get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
{
result = impl_->write( time, period );
if ( result == return_type::ERROR )
{
error( );
}
}
return result;
}
} // namespace hardware_interface
// Copyright 2020 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <tinyxml2.h>
#include <regex>
#include <stdexcept>
#include <string>
#include <unordered_map>
#include <vector>
#include "hardware_interface/component_parser.hpp"
#include "hardware_interface/hardware_info.hpp"
namespace
{
27 constexpr const auto kRobotTag = "robot";
28 constexpr const auto kROS2ControlTag = "ros2_control";
29 constexpr const auto kHardwareTag = "hardware";
30 constexpr const auto kClassTypeTag = "plugin";
constexpr const auto kParamTag = "param";
constexpr const auto kActuatorTag = "actuator";
constexpr const auto kJointTag = "joint";
constexpr const auto kSensorTag = "sensor";
constexpr const auto kGPIOTag = "gpio";
constexpr const auto kTransmissionTag = "transmission";
constexpr const auto kCommandInterfaceTag = "command_interface";
constexpr const auto kStateInterfaceTag = "state_interface";
constexpr const auto kMinTag = "min";
constexpr const auto kMaxTag = "max";
constexpr const auto kInitialValueTag = "initial_value";
constexpr const auto kDataTypeAttribute = "data_type";
constexpr const auto kSizeAttribute = "size";
constexpr const auto kNameAttribute = "name";
constexpr const auto kTypeAttribute = "type";
constexpr const auto kRoleAttribute = "role";
constexpr const auto kReductionAttribute = "mechanical_reduction";
constexpr const auto kOffsetAttribute = "offset";
} // namespace
namespace hardware_interface
{
namespace detail
{
/// Gets value of the text between tags.
/**
* \param[in] element_it XMLElement iterator to search for the text.
* \param[in] tag_name parent tag name where text is searched for ( used for error output )
* \return text of for the tag
* \throws std::runtime_error if text is not found
*/
std::string get_text_for_element(
const tinyxml2::XMLElement * element_it, const std::string & tag_name )
{
const auto get_text_output = element_it->GetText( );
if ( !get_text_output )
{
throw std::runtime_error( "text not specified in the " + tag_name + " tag" );
}
return get_text_output;
}
/// Gets value of the attribute on an XMLelement.
/**
* If attribute is not found throws an error.
*
* \param[in] element_it XMLElement iterator to search for the attribute
* \param[in] attribute_name attribute name to search for and return value
* \param[in] tag_name parent tag name where attribute is searched for ( used for error output )
* \return attribute value
* \throws std::runtime_error if attribute is not found
*/
std::string get_attribute_value(
const tinyxml2::XMLElement * element_it, const char * attribute_name, std::string tag_name )
{
const tinyxml2::XMLAttribute * attr;
attr = element_it->FindAttribute( attribute_name );
if ( !attr )
{
throw std::runtime_error(
"no attribute " + std::string( attribute_name ) + " in " + tag_name + " tag" );
}
return element_it->Attribute( attribute_name );
}
/// Gets value of the attribute on an XMLelement.
/**
* If attribute is not found throws an error.
*
* \param[in] element_it XMLElement iterator to search for the attribute
* \param[in] attribute_name attribute name to search for and return value
* \param[in] tag_name parent tag name where attribute is searched for ( used for error output )
* \return attribute value
* \throws std::runtime_error if attribute is not found
*/
std::string get_attribute_value(
const tinyxml2::XMLElement * element_it, const char * attribute_name, const char * tag_name )
{
return get_attribute_value( element_it, attribute_name, std::string( tag_name ) );
}
/// Gets value of the parameter on an XMLelement.
/**
* If parameter is not found, returns specified default value
*
* \param[in] element_it XMLElement iterator to search for the attribute
* \param[in] attribute_name attribute name to search for and return value
* \param[in] default_value When the attribute is not found, this value is returned instead
* \return attribute value or default
*/
double get_parameter_value_or(
const tinyxml2::XMLElement * params_it, const char * parameter_name, const double default_value )
{
while ( params_it )
{
try
{
// Fill the map with parameters
const auto tag_name = params_it->Name( );
if ( strcmp( tag_name, parameter_name ) == 0 )
{
const auto tag_text = params_it->GetText( );
if ( tag_text )
{
return std::stod( tag_text );
}
}
}
catch ( const std::exception & e )
{
return default_value;
}
params_it = params_it->NextSiblingElement( );
}
return default_value;
}
/// Parse optional size attribute
/**
* Parses an XMLElement and returns the value of the size attribute.
* If not specified, defaults to 1. If not given a positive integer, throws an error.
*
* \param[in] elem XMLElement that has the size attribute.
* \return The size.
* \throws std::runtime_error if not given a positive non-zero integer as value.
*/
std::size_t parse_size_attribute( const tinyxml2::XMLElement * elem )
{
const tinyxml2::XMLAttribute * attr = elem->FindAttribute( kSizeAttribute );
if ( !attr )
{
return 1;
}
std::size_t size;
// Regex used to check for non-zero positive int
std::string s = attr->Value( );
std::regex int_re( "[1-9][0-9]*" );
if ( std::regex_match( s, int_re ) )
{
size = std::stoi( s );
}
else
{
throw std::runtime_error(
"Could not parse size tag in \"" + std::string( elem->Name( ) ) + "\"." + "Got \"" + s +
"\", but expected a non-zero positive integer." );
}
return size;
}
/// Parse data_type attribute
/**
* Parses an XMLElement and returns the value of the data_type attribute.
* Defaults to "double" if not specified.
*
* \param[in] elem XMLElement that has the data_type attribute.
* \return string specifying the data type.
*/
std::string parse_data_type_attribute( const tinyxml2::XMLElement * elem )
{
const tinyxml2::XMLAttribute * attr = elem->FindAttribute( kDataTypeAttribute );
std::string data_type;
if ( !attr )
{
data_type = "double";
}
else
{
data_type = attr->Value( );
}
return data_type;
}
/// Search XML snippet from URDF for parameters.
/**
* \param[in] params_it pointer to the iterator where parameters info should be found
* \return key-value map with parameters
* \throws std::runtime_error if a component attribute or tag is not found
*/
std::unordered_map<std::string, std::string> parse_parameters_from_xml(
const tinyxml2::XMLElement * params_it )
{
std::unordered_map<std::string, std::string> parameters;
const tinyxml2::XMLAttribute * attr;
while ( params_it )
{
// Fill the map with parameters
attr = params_it->FindAttribute( kNameAttribute );
if ( !attr )
{
throw std::runtime_error( "no parameter name attribute set in param tag" );
}
const std::string parameter_name = params_it->Attribute( kNameAttribute );
const std::string parameter_value = get_text_for_element( params_it, parameter_name );
parameters[parameter_name] = parameter_value;
params_it = params_it->NextSiblingElement( kParamTag );
}
return parameters;
}
/// Search XML snippet for definition of interfaceTypes.
/**
* \param[in] interfaces_it pointer to the iterator over interfaces
* \param[in] interfaceTag interface type tag ( command or state )
* \return list of interface types
* \throws std::runtime_error if the interfaceType text not set in a tag
*/
hardware_interface::InterfaceInfo parse_interfaces_from_xml(
const tinyxml2::XMLElement * interfaces_it )
{
hardware_interface::InterfaceInfo interface;
const std::string interface_name =
get_attribute_value( interfaces_it, kNameAttribute, interfaces_it->Name( ) );
interface.name = interface_name;
// Optional min/max attributes
std::unordered_map<std::string, std::string> interface_params =
parse_parameters_from_xml( interfaces_it->FirstChildElement( kParamTag ) );
auto interface_param = interface_params.find( kMinTag );
if ( interface_param != interface_params.end( ) )
{
interface.min = interface_param->second;
}
interface_param = interface_params.find( kMaxTag );
if ( interface_param != interface_params.end( ) )
{
interface.max = interface_param->second;
}
// Optional initial_value attribute
interface_param = interface_params.find( kInitialValueTag );
if ( interface_param != interface_params.end( ) )
{
interface.initial_value = interface_param->second;
}
// Default to a single double
interface.data_type = "double";
interface.size = 1;
return interface;
}
/// Search XML snippet from URDF for information about a control component.
/**
* \param[in] component_it pointer to the iterator where component
* info should be found
* \return ComponentInfo filled with information about component
* \throws std::runtime_error if a component attribute or tag is not found
*/
ComponentInfo parse_component_from_xml( const tinyxml2::XMLElement * component_it )
{
ComponentInfo component;
// Find name, type and class of a component
component.type = component_it->Name( );
component.name = get_attribute_value( component_it, kNameAttribute, component.type );
// Parse all command interfaces
const auto * command_interfaces_it = component_it->FirstChildElement( kCommandInterfaceTag );
while ( command_interfaces_it )
{
component.command_interfaces.push_back( parse_interfaces_from_xml( command_interfaces_it ) );
command_interfaces_it = command_interfaces_it->NextSiblingElement( kCommandInterfaceTag );
}
// Parse state interfaces
const auto * state_interfaces_it = component_it->FirstChildElement( kStateInterfaceTag );
while ( state_interfaces_it )
{
component.state_interfaces.push_back( parse_interfaces_from_xml( state_interfaces_it ) );
state_interfaces_it = state_interfaces_it->NextSiblingElement( kStateInterfaceTag );
}
// Parse parameters
const auto * params_it = component_it->FirstChildElement( kParamTag );
if ( params_it )
{
component.parameters = parse_parameters_from_xml( params_it );
}
return component;
}
/// Search XML snippet from URDF for information about a complex component.
/**
* A complex component can have a non-double data type specified on its interfaces,
* and the interface may be an array of a fixed size of the data type.
*
* \param[in] component_it pointer to the iterator where component
* info should befound
* \throws std::runtime_error if a required component attribute or tag is not found.
*/
ComponentInfo parse_complex_component_from_xml( const tinyxml2::XMLElement * component_it )
{
ComponentInfo component;
// Find name, type and class of a component
component.type = component_it->Name( );
component.name = get_attribute_value( component_it, kNameAttribute, component.type );
// Parse all command interfaces
const auto * command_interfaces_it = component_it->FirstChildElement( kCommandInterfaceTag );
while ( command_interfaces_it )
{
component.command_interfaces.push_back( parse_interfaces_from_xml( command_interfaces_it ) );
component.command_interfaces.back( ).data_type =
parse_data_type_attribute( command_interfaces_it );
component.command_interfaces.back( ).size = parse_size_attribute( command_interfaces_it );
command_interfaces_it = command_interfaces_it->NextSiblingElement( kCommandInterfaceTag );
}
// Parse state interfaces
const auto * state_interfaces_it = component_it->FirstChildElement( kStateInterfaceTag );
while ( state_interfaces_it )
{
component.state_interfaces.push_back( parse_interfaces_from_xml( state_interfaces_it ) );
component.state_interfaces.back( ).data_type = parse_data_type_attribute( state_interfaces_it );
component.state_interfaces.back( ).size = parse_size_attribute( state_interfaces_it );
state_interfaces_it = state_interfaces_it->NextSiblingElement( kStateInterfaceTag );
}
// Parse parameters
const auto * params_it = component_it->FirstChildElement( kParamTag );
if ( params_it )
{
component.parameters = parse_parameters_from_xml( params_it );
}
return component;
}
JointInfo parse_transmission_joint_from_xml( const tinyxml2::XMLElement * element_it )
{
JointInfo joint_info;
joint_info.name = get_attribute_value( element_it, kNameAttribute, element_it->Name( ) );
joint_info.role = get_attribute_value( element_it, kRoleAttribute, element_it->Name( ) );
joint_info.mechanical_reduction =
get_parameter_value_or( element_it->FirstChildElement( ), kReductionAttribute, 1.0 );
joint_info.offset =
get_parameter_value_or( element_it->FirstChildElement( ), kOffsetAttribute, 0.0 );
return joint_info;
}
ActuatorInfo parse_transmission_actuator_from_xml( const tinyxml2::XMLElement * element_it )
{
ActuatorInfo actuator_info;
actuator_info.name = get_attribute_value( element_it, kNameAttribute, element_it->Name( ) );
actuator_info.role = get_attribute_value( element_it, kRoleAttribute, element_it->Name( ) );
actuator_info.mechanical_reduction =
get_parameter_value_or( element_it->FirstChildElement( ), kReductionAttribute, 1.0 );
actuator_info.offset =
get_parameter_value_or( element_it->FirstChildElement( ), kOffsetAttribute, 0.0 );
return actuator_info;
}
/// Search XML snippet from URDF for information about a transmission.
/**
* \param[in] transmission_it pointer to the iterator where transmission info should be found
* \return TransmissionInfo filled with information about transmission
* \throws std::runtime_error if an attribute or tag is not found
*/
TransmissionInfo parse_transmission_from_xml( const tinyxml2::XMLElement * transmission_it )
{
TransmissionInfo transmission;
// Find name, type and class of a transmission
transmission.name = get_attribute_value( transmission_it, kNameAttribute, transmission_it->Name( ) );
const auto * type_it = transmission_it->FirstChildElement( kClassTypeTag );
transmission.type = get_text_for_element( type_it, kClassTypeTag );
// Parse joints
const auto * joint_it = transmission_it->FirstChildElement( kJointTag );
while ( joint_it )
{
transmission.joints.push_back( parse_transmission_joint_from_xml( joint_it ) );
joint_it = joint_it->NextSiblingElement( kJointTag );
}
// Parse actuators
const auto * actuator_it = transmission_it->FirstChildElement( kActuatorTag );
while ( actuator_it )
{
transmission.actuators.push_back( parse_transmission_actuator_from_xml( actuator_it ) );
actuator_it = actuator_it->NextSiblingElement( kActuatorTag );
}
// Parse parameters
const auto * params_it = transmission_it->FirstChildElement( kParamTag );
if ( params_it )
{
transmission.parameters = parse_parameters_from_xml( params_it );
}
return transmission;
}
/// Auto-fill some contents of transmission info based on context
/**
* \param[in, out] hardware HardwareInfo structure with elements already parsed.
* \throws std::runtime_error
*/
void auto_fill_transmission_interfaces( HardwareInfo & hardware )
{
for ( auto & transmission : hardware.transmissions )
{
// fill joint interfaces for actuator from joints declared for this component
for ( auto & joint : transmission.joints )
{
auto found_it = std::find_if(
hardware.joints.cbegin( ), hardware.joints.cend( ),
[&joint]( const auto & joint_info ) { return joint.name == joint_info.name; } );
if ( found_it == hardware.joints.cend( ) )
{
throw std::runtime_error(
"Error while parsing '" + hardware.name + "'. Transmission '" + transmission.name +
"' declared joint '" + joint.name + "' is not available in component '" + hardware.name +
"'." );
}
// copy interface names from their definitions in the component
std::transform(
found_it->command_interfaces.cbegin( ), found_it->command_interfaces.cend( ),
std::back_inserter( joint.interfaces ),
[]( const auto & interface ) { return interface.name; } );
}
// we parsed an actuator component, here we fill in more details
if ( hardware.type == kActuatorTag )
{
if ( transmission.joints.size( ) != 1 )
{
throw std::runtime_error(
"Error while parsing '" + hardware.name +
"'. There should be exactly one joint defined in this component but found " +
std::to_string( transmission.joints.size( ) ) );
}
transmission.actuators.push_back(
ActuatorInfo{"actuator1", transmission.joints[0].interfaces, "actuator1", 1.0, 0.0} );
}
}
}
/// Parse a control resource from an "ros2_control" tag.
/**
* \param[in] ros2_control_it pointer to ros2_control element
* with information about resource.
* \return HardwareInfo filled with information about the robot
* \throws std::runtime_error if a attributes or tag are not found
*/
HardwareInfo parse_resource_from_xml(
const tinyxml2::XMLElement * ros2_control_it, const std::string & urdf )
{
HardwareInfo hardware;
hardware.name = get_attribute_value( ros2_control_it, kNameAttribute, kROS2ControlTag );
hardware.type = get_attribute_value( ros2_control_it, kTypeAttribute, kROS2ControlTag );
// Parse everything under ros2_control tag
hardware.hardware_class_type = "";
const auto * ros2_control_child_it = ros2_control_it->FirstChildElement( );
while ( ros2_control_child_it )
{
if ( !std::string( kHardwareTag ).compare( ros2_control_child_it->Name( ) ) )
{
const auto * type_it = ros2_control_child_it->FirstChildElement( kClassTypeTag );
hardware.hardware_class_type =
get_text_for_element( type_it, std::string( "hardware " ) + kClassTypeTag );
const auto * params_it = ros2_control_child_it->FirstChildElement( kParamTag );
if ( params_it )
{
hardware.hardware_parameters = parse_parameters_from_xml( params_it );
}
}
else if ( !std::string( kJointTag ).compare( ros2_control_child_it->Name( ) ) )
{
hardware.joints.push_back( parse_component_from_xml( ros2_control_child_it ) );
}
else if ( !std::string( kSensorTag ).compare( ros2_control_child_it->Name( ) ) )
{
hardware.sensors.push_back( parse_component_from_xml( ros2_control_child_it ) );
}
else if ( !std::string( kGPIOTag ).compare( ros2_control_child_it->Name( ) ) )
{
hardware.gpios.push_back( parse_complex_component_from_xml( ros2_control_child_it ) );
}
else if ( !std::string( kTransmissionTag ).compare( ros2_control_child_it->Name( ) ) )
{
hardware.transmissions.push_back( parse_transmission_from_xml( ros2_control_child_it ) );
}
else
{
throw std::runtime_error( "invalid tag name " + std::string( ros2_control_child_it->Name( ) ) );
}
ros2_control_child_it = ros2_control_child_it->NextSiblingElement( );
}
auto_fill_transmission_interfaces( hardware );
hardware.original_xml = urdf;
return hardware;
}
} // namespace detail
std::vector<HardwareInfo> parse_control_resources_from_urdf( const std::string & urdf )
{
// Check if everything OK with URDF string
if ( urdf.empty( ) )
{
throw std::runtime_error( "empty URDF passed to robot" );
}
tinyxml2::XMLDocument doc;
if ( !doc.Parse( urdf.c_str( ) ) && doc.Error( ) )
{
throw std::runtime_error( "invalid URDF passed in to robot parser" );
}
if ( doc.Error( ) )
{
throw std::runtime_error( "invalid URDF passed in to robot parser" );
}
// Find robot tag
const tinyxml2::XMLElement * robot_it = doc.RootElement( );
if ( std::string( kRobotTag ).compare( robot_it->Name( ) ) )
{
throw std::runtime_error( "the robot tag is not root element in URDF" );
}
const tinyxml2::XMLElement * ros2_control_it = robot_it->FirstChildElement( kROS2ControlTag );
if ( !ros2_control_it )
{
throw std::runtime_error( "no " + std::string( kROS2ControlTag ) + " tag" );
}
std::vector<HardwareInfo> hardware_info;
while ( ros2_control_it )
{
hardware_info.push_back( detail::parse_resource_from_xml( ros2_control_it, urdf ) );
ros2_control_it = ros2_control_it->NextSiblingElement( kROS2ControlTag );
}
return hardware_info;
}
} // namespace hardware_interface
1 // Copyright ( c ) 2022 PickNik, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Author: Jafar Abdi, Denis Stogl
#include "fake_components/generic_system.hpp"
#include "pluginlib/class_list_macros.hpp"
19 PLUGINLIB_EXPORT_CLASS( fake_components::GenericSystem, hardware_interface::SystemInterface )
1 // Copyright ( c ) 2021 PickNik, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Author: Jafar Abdi, Denis Stogl
#include "mock_components/generic_system.hpp"
#include <algorithm>
#include <cmath>
#include <iterator>
#include <limits>
#include <set>
#include <string>
#include <vector>
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rcutils/logging_macros.h"
namespace mock_components
{
32 CallbackReturn GenericSystem::on_init( const hardware_interface::HardwareInfo & info )
{
if ( hardware_interface::SystemInterface::on_init( info ) != CallbackReturn::SUCCESS )
{
return CallbackReturn::ERROR;
}
auto populate_non_standard_interfaces =
[this]( auto interface_list, auto & non_standard_interfaces )
{
for ( const auto & interface : interface_list )
{
// add to list if non-standard interface
if (
std::find( standard_interfaces_.begin( ), standard_interfaces_.end( ), interface.name ) ==
standard_interfaces_.end( ) )
{
if (
std::find(
non_standard_interfaces.begin( ), non_standard_interfaces.end( ), interface.name ) ==
non_standard_interfaces.end( ) )
{
non_standard_interfaces.emplace_back( interface.name );
}
}
}
};
// check if to create fake command interface for sensor
auto it = info_.hardware_parameters.find( "fake_sensor_commands" );
if ( it != info_.hardware_parameters.end( ) )
{
// TODO( anyone ): change this to parse_bool( ) ( see ros2_control#339 )
use_fake_sensor_command_interfaces_ = it->second == "true" || it->second == "True";
}
else
{
use_fake_sensor_command_interfaces_ = false;
}
// check if to create fake command interface for gpio
it = info_.hardware_parameters.find( "fake_gpio_commands" );
if ( it != info_.hardware_parameters.end( ) )
{
// TODO( anyone ): change this to parse_bool( ) ( see ros2_control#339 )
use_fake_gpio_command_interfaces_ = it->second == "true" || it->second == "True";
}
else
{
use_fake_gpio_command_interfaces_ = false;
}
// process parameters about state following
position_state_following_offset_ = 0.0;
custom_interface_with_following_offset_ = "";
it = info_.hardware_parameters.find( "position_state_following_offset" );
if ( it != info_.hardware_parameters.end( ) )
{
position_state_following_offset_ = std::stod( it->second );
it = info_.hardware_parameters.find( "custom_interface_with_following_offset" );
if ( it != info_.hardware_parameters.end( ) )
{
custom_interface_with_following_offset_ = it->second;
}
}
// its extremlly unprobably that std::distance results int this value - therefore default
index_custom_interface_with_following_offset_ = std::numeric_limits<size_t>::max( );
// Initialize storage for standard interfaces
initialize_storage_vectors( joint_commands_, joint_states_, standard_interfaces_ );
// set all values without initial values to 0
for ( auto i = 0u; i < info_.joints.size( ); i++ )
{
for ( auto j = 0u; j < standard_interfaces_.size( ); j++ )
{
if ( std::isnan( joint_states_[j][i] ) )
{
joint_states_[j][i] = 0.0;
}
}
}
// Search for mimic joints
for ( auto i = 0u; i < info_.joints.size( ); ++i )
{
const auto & joint = info_.joints.at( i );
if ( joint.parameters.find( "mimic" ) != joint.parameters.cend( ) )
{
const auto mimicked_joint_it = std::find_if(
info_.joints.begin( ), info_.joints.end( ),
[&mimicked_joint =
joint.parameters.at( "mimic" )]( const hardware_interface::ComponentInfo & joint_info )
{ return joint_info.name == mimicked_joint; } );
if ( mimicked_joint_it == info_.joints.cend( ) )
{
throw std::runtime_error(
std::string( "Mimicked joint '" ) + joint.parameters.at( "mimic" ) + "' not found" );
}
MimicJoint mimic_joint;
mimic_joint.joint_index = i;
mimic_joint.mimicked_joint_index = std::distance( info_.joints.begin( ), mimicked_joint_it );
auto param_it = joint.parameters.find( "multiplier" );
if ( param_it != joint.parameters.end( ) )
{
mimic_joint.multiplier = std::stod( joint.parameters.at( "multiplier" ) );
}
mimic_joints_.push_back( mimic_joint );
}
}
// search for non-standard joint interfaces
for ( const auto & joint : info_.joints )
{
// populate non-standard command interfaces to other_interfaces_
populate_non_standard_interfaces( joint.command_interfaces, other_interfaces_ );
// populate non-standard state interfaces to other_interfaces_
populate_non_standard_interfaces( joint.state_interfaces, other_interfaces_ );
}
// Initialize storage for non-standard interfaces
initialize_storage_vectors( other_commands_, other_states_, other_interfaces_ );
// when following offset is used on custom interface then find its index
if ( !custom_interface_with_following_offset_.empty( ) )
{
auto if_it = std::find(
other_interfaces_.begin( ), other_interfaces_.end( ), custom_interface_with_following_offset_ );
if ( if_it != other_interfaces_.end( ) )
{
index_custom_interface_with_following_offset_ =
std::distance( other_interfaces_.begin( ), if_it );
RCUTILS_LOG_INFO_NAMED(
"fake_generic_system", "Custom interface with following offset '%s' found at index: %zu.",
custom_interface_with_following_offset_.c_str( ),
index_custom_interface_with_following_offset_ );
}
else
{
RCUTILS_LOG_WARN_NAMED(
"fake_generic_system",
"Custom interface with following offset '%s' does not exist. Offset will not be applied",
custom_interface_with_following_offset_.c_str( ) );
}
}
for ( const auto & sensor : info_.sensors )
{
for ( const auto & interface : sensor.state_interfaces )
{
if (
std::find( sensor_interfaces_.begin( ), sensor_interfaces_.end( ), interface.name ) ==
sensor_interfaces_.end( ) )
{
sensor_interfaces_.emplace_back( interface.name );
}
}
}
initialize_storage_vectors( sensor_fake_commands_, sensor_states_, sensor_interfaces_ );
// search for gpio interfaces
for ( const auto & gpio : info_.gpios )
{
// populate non-standard command interfaces to gpio_interfaces_
populate_non_standard_interfaces( gpio.command_interfaces, gpio_interfaces_ );
// populate non-standard state interfaces to gpio_interfaces_
populate_non_standard_interfaces( gpio.state_interfaces, gpio_interfaces_ );
}
// Fake gpio command interfaces
if ( use_fake_gpio_command_interfaces_ )
{
initialize_storage_vectors( gpio_fake_commands_, gpio_states_, gpio_interfaces_ );
}
// Real gpio command interfaces
else
{
initialize_storage_vectors( gpio_commands_, gpio_states_, gpio_interfaces_ );
}
return CallbackReturn::SUCCESS;
}
217 std::vector<hardware_interface::StateInterface> GenericSystem::export_state_interfaces( )
{
std::vector<hardware_interface::StateInterface> state_interfaces;
// Joints' state interfaces
for ( auto i = 0u; i < info_.joints.size( ); i++ )
{
const auto & joint = info_.joints[i];
for ( const auto & interface : joint.state_interfaces )
{
// Add interface: if not in the standard list then use "other" interface list
if ( !get_interface(
joint.name, standard_interfaces_, interface.name, i, joint_states_, state_interfaces ) )
{
if ( !get_interface(
joint.name, other_interfaces_, interface.name, i, other_states_, state_interfaces ) )
{
throw std::runtime_error(
"Interface is not found in the standard nor other list. "
"This should never happen!" );
}
}
}
}
// Sensor state interfaces
if ( !populate_interfaces(
info_.sensors, sensor_interfaces_, sensor_states_, state_interfaces, true ) )
{
throw std::runtime_error(
"Interface is not found in the standard nor other list. This should never happen!" );
};
// GPIO state interfaces
if ( !populate_interfaces( info_.gpios, gpio_interfaces_, gpio_states_, state_interfaces, true ) )
{
throw std::runtime_error( "Interface is not found in the gpio list. This should never happen!" );
}
return state_interfaces;
}
259 std::vector<hardware_interface::CommandInterface> GenericSystem::export_command_interfaces( )
{
std::vector<hardware_interface::CommandInterface> command_interfaces;
// Joints' state interfaces
for ( auto i = 0u; i < info_.joints.size( ); i++ )
{
const auto & joint = info_.joints[i];
for ( const auto & interface : joint.command_interfaces )
{
// Add interface: if not in the standard list than use "other" interface list
if ( !get_interface(
joint.name, standard_interfaces_, interface.name, i, joint_commands_,
command_interfaces ) )
{
if ( !get_interface(
joint.name, other_interfaces_, interface.name, i, other_commands_,
command_interfaces ) )
{
throw std::runtime_error(
"Interface is not found in the standard nor other list. "
"This should never happen!" );
}
}
}
}
// Fake sensor command interfaces
if ( use_fake_sensor_command_interfaces_ )
{
if ( !populate_interfaces(
info_.sensors, sensor_interfaces_, sensor_fake_commands_, command_interfaces, true ) )
{
throw std::runtime_error(
"Interface is not found in the standard nor other list. This should never happen!" );
}
}
// Fake gpio command interfaces ( consider all state interfaces for command interfaces )
if ( use_fake_gpio_command_interfaces_ )
{
if ( !populate_interfaces(
info_.gpios, gpio_interfaces_, gpio_fake_commands_, command_interfaces, true ) )
{
throw std::runtime_error(
"Interface is not found in the gpio list. This should never happen!" );
}
}
// GPIO command interfaces ( real command interfaces )
else
{
if ( !populate_interfaces(
info_.gpios, gpio_interfaces_, gpio_commands_, command_interfaces, false ) )
{
throw std::runtime_error(
"Interface is not found in the gpio list. This should never happen!" );
}
}
return command_interfaces;
}
321 return_type GenericSystem::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ )
{
auto mirror_command_to_state = []( auto & states_, auto commands_, size_t start_index = 0 )
{
for ( size_t i = start_index; i < states_.size( ); ++i )
{
for ( size_t j = 0; j < states_[i].size( ); ++j )
{
if ( !std::isnan( commands_[i][j] ) )
{
states_[i][j] = commands_[i][j];
}
}
}
};
// apply offset to positions only
for ( size_t j = 0; j < joint_states_[POSITION_INTERFACE_INDEX].size( ); ++j )
{
if ( !std::isnan( joint_commands_[POSITION_INTERFACE_INDEX][j] ) )
{
joint_states_[POSITION_INTERFACE_INDEX][j] =
joint_commands_[POSITION_INTERFACE_INDEX][j] +
( custom_interface_with_following_offset_.empty( ) ? position_state_following_offset_ : 0.0 );
}
}
// do loopback on all other interfaces - starts from 1 because 0 index is position interface
mirror_command_to_state( joint_states_, joint_commands_, 1 );
for ( const auto & mimic_joint : mimic_joints_ )
{
for ( auto i = 0u; i < joint_states_.size( ); ++i )
{
joint_states_[i][mimic_joint.joint_index] =
mimic_joint.multiplier * joint_states_[i][mimic_joint.mimicked_joint_index];
}
}
for ( size_t i = 0; i < other_states_.size( ); ++i )
{
for ( size_t j = 0; j < other_states_[i].size( ); ++j )
{
if (
i == index_custom_interface_with_following_offset_ &&
!std::isnan( joint_commands_[POSITION_INTERFACE_INDEX][j] ) )
{
other_states_[i][j] =
joint_commands_[POSITION_INTERFACE_INDEX][j] + position_state_following_offset_;
}
else if ( !std::isnan( other_commands_[i][j] ) )
{
other_states_[i][j] = other_commands_[i][j];
}
}
}
if ( use_fake_sensor_command_interfaces_ )
{
mirror_command_to_state( sensor_states_, sensor_fake_commands_ );
}
// do loopback on all gpio interfaces
if ( use_fake_gpio_command_interfaces_ )
{
mirror_command_to_state( gpio_states_, gpio_fake_commands_ );
}
else
{
mirror_command_to_state( gpio_states_, gpio_commands_ );
}
return return_type::OK;
}
// Private methods
template <typename HandleType>
398 bool GenericSystem::get_interface(
const std::string & name, const std::vector<std::string> & interface_list,
const std::string & interface_name, const size_t vector_index,
std::vector<std::vector<double>> & values, std::vector<HandleType> & interfaces )
{
auto it = std::find( interface_list.begin( ), interface_list.end( ), interface_name );
if ( it != interface_list.end( ) )
{
auto j = std::distance( interface_list.begin( ), it );
interfaces.emplace_back( name, *it, &values[j][vector_index] );
return true;
}
return false;
}
413 void GenericSystem::initialize_storage_vectors(
std::vector<std::vector<double>> & commands, std::vector<std::vector<double>> & states,
const std::vector<std::string> & interfaces )
{
// Initialize storage for all joints, regardless of their existence
commands.resize( interfaces.size( ) );
states.resize( interfaces.size( ) );
for ( auto i = 0u; i < interfaces.size( ); i++ )
{
commands[i].resize( info_.joints.size( ), std::numeric_limits<double>::quiet_NaN( ) );
states[i].resize( info_.joints.size( ), std::numeric_limits<double>::quiet_NaN( ) );
}
// Initialize with values from URDF
bool print_hint = false;
for ( auto i = 0u; i < info_.joints.size( ); i++ )
{
const auto & joint = info_.joints[i];
for ( const auto & interface : joint.state_interfaces )
{
auto it = std::find( interfaces.begin( ), interfaces.end( ), interface.name );
// If interface name is found in the interfaces list
if ( it != interfaces.end( ) )
{
auto index = std::distance( interfaces.begin( ), it );
// Check the initial_value param is used
if ( !interface.initial_value.empty( ) )
{
states[index][i] = std::stod( interface.initial_value );
}
else
{
// Initialize the value in old way with warning message
auto it2 = joint.parameters.find( "initial_" + interface.name );
if ( it2 != joint.parameters.end( ) )
{
states[index][i] = std::stod( it2->second );
print_hint = true;
}
else
{
print_hint = true;
}
}
}
}
}
if ( print_hint )
{
RCUTILS_LOG_WARN_ONCE_NAMED(
"mock_generic_system",
"Parsing of optional initial interface values failed or uses a deprecated format. Add "
"initial values for every state interface in the ros2_control.xacro. For example: \n"
"<state_interface name=\"velocity\"> \n"
" <param name=\"initial_value\">0.0</param> \n"
"</state_interface>" );
}
}
template <typename InterfaceType>
475 bool GenericSystem::populate_interfaces(
const std::vector<hardware_interface::ComponentInfo> & components,
std::vector<std::string> & interface_names, std::vector<std::vector<double>> & storage,
std::vector<InterfaceType> & target_interfaces, bool using_state_interfaces )
{
for ( auto i = 0u; i < components.size( ); i++ )
{
const auto & component = components[i];
const auto interfaces =
( using_state_interfaces ) ? component.state_interfaces : component.command_interfaces;
for ( const auto & interface : interfaces )
{
if ( !get_interface(
component.name, interface_names, interface.name, i, storage, target_interfaces ) )
{
return false;
}
}
}
return true;
}
} // namespace mock_components
#include "pluginlib/class_list_macros.hpp"
500 PLUGINLIB_EXPORT_CLASS( mock_components::GenericSystem, hardware_interface::SystemInterface )
// Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "hardware_interface/resource_manager.hpp"
#include <functional>
#include <map>
#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>
#include "hardware_interface/actuator.hpp"
#include "hardware_interface/actuator_interface.hpp"
#include "hardware_interface/component_parser.hpp"
#include "hardware_interface/hardware_component_info.hpp"
#include "hardware_interface/sensor.hpp"
#include "hardware_interface/sensor_interface.hpp"
#include "hardware_interface/system.hpp"
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "pluginlib/class_loader.hpp"
#include "rcutils/logging_macros.h"
namespace hardware_interface
{
40 auto trigger_and_print_hardware_state_transition =
[](
auto transition, const std::string transition_name, const std::string & hardware_name,
const lifecycle_msgs::msg::State::_id_type & target_state )
{
RCUTILS_LOG_INFO_NAMED(
"resource_manager", "'%s' hardware '%s' ", transition_name.c_str( ), hardware_name.c_str( ) );
const rclcpp_lifecycle::State new_state = transition( );
bool result = new_state.id( ) == target_state;
if ( result )
{
RCUTILS_LOG_INFO_NAMED(
"resource_manager", "Successful '%s' of hardware '%s'", transition_name.c_str( ),
hardware_name.c_str( ) );
}
else
{
RCUTILS_LOG_INFO_NAMED(
"resource_manager", "Failed to '%s' hardware '%s'", transition_name.c_str( ),
hardware_name.c_str( ) );
}
return result;
};
67 class ResourceStorage
{
static constexpr const char * pkg_name = "hardware_interface";
71 static constexpr const char * actuator_interface_name = "hardware_interface::ActuatorInterface";
72 static constexpr const char * sensor_interface_name = "hardware_interface::SensorInterface";
73 static constexpr const char * system_interface_name = "hardware_interface::SystemInterface";
public:
ResourceStorage( )
: actuator_loader_( pkg_name, actuator_interface_name ),
sensor_loader_( pkg_name, sensor_interface_name ),
system_loader_( pkg_name, system_interface_name )
{
}
template <class HardwareT, class HardwareInterfaceT>
void load_hardware(
const HardwareInfo & hardware_info, pluginlib::ClassLoader<HardwareInterfaceT> & loader,
std::vector<HardwareT> & container )
{
RCUTILS_LOG_INFO_NAMED(
"resource_manager", "Loading hardware '%s' ", hardware_info.name.c_str( ) );
// hardware_class_type has to match class name in plugin xml description
// TODO( karsten1987 ) extract package from hardware_class_type
// e.g.: <package_vendor>/<system_type>
auto interface = std::unique_ptr<HardwareInterfaceT>(
loader.createUnmanagedInstance( hardware_info.hardware_class_type ) );
HardwareT hardware( std::move( interface ) );
container.emplace_back( std::move( hardware ) );
// initialize static data about hardware component to reduce later calls
HardwareComponentInfo component_info;
component_info.name = hardware_info.name;
component_info.type = hardware_info.type;
component_info.class_type = hardware_info.hardware_class_type;
hardware_info_map_.insert( std::make_pair( component_info.name, component_info ) );
}
template <class HardwareT>
bool initialize_hardware( const HardwareInfo & hardware_info, HardwareT & hardware )
{
RCUTILS_LOG_INFO_NAMED(
"resource_manager", "Initialize hardware '%s' ", hardware_info.name.c_str( ) );
const rclcpp_lifecycle::State new_state = hardware.initialize( hardware_info );
bool result = new_state.id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED;
if ( result )
{
RCUTILS_LOG_INFO_NAMED(
"resource_manager", "Successful initialization of hardware '%s'",
hardware_info.name.c_str( ) );
}
else
{
RCUTILS_LOG_INFO_NAMED(
"resource_manager", "Failed to initialize hardware '%s'", hardware_info.name.c_str( ) );
}
return result;
}
template <class HardwareT>
bool configure_hardware( HardwareT & hardware )
{
bool result = trigger_and_print_hardware_state_transition(
std::bind( &HardwareT::configure, &hardware ), "configure", hardware.get_name( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
if ( result )
{
// TODO( destogl ): is it better to check here if previous state was unconfigured instead of
// checking if each state already exists? Or we should somehow know that transition has
// happened and only then trigger this part of the code?
// On the other side this part of the code should never be executed in real-time critical
// thread, so it could be also OK as it is...
for ( const auto & interface : hardware_info_map_[hardware.get_name( )].state_interfaces )
{
// add all state interfaces to available list
auto found_it = std::find(
available_state_interfaces_.begin( ), available_state_interfaces_.end( ), interface );
if ( found_it == available_state_interfaces_.end( ) )
{
available_state_interfaces_.emplace_back( interface );
RCUTILS_LOG_DEBUG_NAMED(
"resource_manager", "( hardware '%s' ): '%s' state interface added into available list",
hardware.get_name( ).c_str( ), interface.c_str( ) );
}
else
{
// TODO( destogl ): do here error management if interfaces are only partially added into
// "available" list - this should never be the case!
RCUTILS_LOG_WARN_NAMED(
"resource_manager",
"( hardware '%s' ): '%s' state interface already in available list."
" This can happen due to multiple calls to 'configure'",
hardware.get_name( ).c_str( ), interface.c_str( ) );
}
}
// add command interfaces to available list
for ( const auto & interface : hardware_info_map_[hardware.get_name( )].command_interfaces )
{
// TODO( destogl ): check if interface should be available on configure
auto found_it = std::find(
available_command_interfaces_.begin( ), available_command_interfaces_.end( ), interface );
if ( found_it == available_command_interfaces_.end( ) )
{
available_command_interfaces_.emplace_back( interface );
RCUTILS_LOG_DEBUG_NAMED(
"resource_manager", "( hardware '%s' ): '%s' command interface added into available list",
hardware.get_name( ).c_str( ), interface.c_str( ) );
}
else
{
// TODO( destogl ): do here error management if interfaces are only partially added into
// "available" list - this should never be the case!
RCUTILS_LOG_WARN_NAMED(
"resource_manager",
"( hardware '%s' ): '%s' command interface already in available list."
" This can happen due to multiple calls to 'configure'",
hardware.get_name( ).c_str( ), interface.c_str( ) );
}
}
}
return result;
}
template <class HardwareT>
bool cleanup_hardware( HardwareT & hardware )
{
bool result = trigger_and_print_hardware_state_transition(
std::bind( &HardwareT::cleanup, &hardware ), "cleanup", hardware.get_name( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
if ( result )
{
// remove all command interfaces from available list
for ( const auto & interface : hardware_info_map_[hardware.get_name( )].command_interfaces )
{
auto found_it = std::find(
available_command_interfaces_.begin( ), available_command_interfaces_.end( ), interface );
if ( found_it != available_command_interfaces_.end( ) )
{
available_command_interfaces_.erase( found_it );
RCUTILS_LOG_DEBUG_NAMED(
"resource_manager",
"( hardware '%s' ): '%s' command interface removed from available list",
hardware.get_name( ).c_str( ), interface.c_str( ) );
}
else
{
// TODO( destogl ): do here error management if interfaces are only partially added into
// "available" list - this should never be the case!
RCUTILS_LOG_WARN_NAMED(
"resource_manager",
"( hardware '%s' ): '%s' command interface not in available list."
" This can happen due to multiple calls to 'cleanup'",
hardware.get_name( ).c_str( ), interface.c_str( ) );
}
}
// remove all state interfaces from available list
for ( const auto & interface : hardware_info_map_[hardware.get_name( )].state_interfaces )
{
auto found_it = std::find(
available_state_interfaces_.begin( ), available_state_interfaces_.end( ), interface );
if ( found_it != available_state_interfaces_.end( ) )
{
available_state_interfaces_.erase( found_it );
RCUTILS_LOG_DEBUG_NAMED(
"resource_manager", "( hardware '%s' ): '%s' state interface removed from available list",
hardware.get_name( ).c_str( ), interface.c_str( ) );
}
else
{
// TODO( destogl ): do here error management if interfaces are only partially added into
// "available" list - this should never be the case!
RCUTILS_LOG_WARN_NAMED(
"resource_manager",
"( hardware '%s' ): '%s' state interface not in available list. "
"This can happen due to multiple calls to 'cleanup'",
hardware.get_name( ).c_str( ), interface.c_str( ) );
}
}
}
return result;
}
template <class HardwareT>
bool shutdown_hardware( HardwareT & hardware )
{
bool result = trigger_and_print_hardware_state_transition(
std::bind( &HardwareT::shutdown, &hardware ), "shutdown", hardware.get_name( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
if ( result )
{
// TODO( destogl ): change this - deimport all things if there is there are interfaces there
// deimport_non_movement_command_interfaces( hardware );
// deimport_state_interfaces( hardware );
// use remove_command_interfaces( hardware );
}
return result;
}
template <class HardwareT>
bool activate_hardware( HardwareT & hardware )
{
bool result = trigger_and_print_hardware_state_transition(
std::bind( &HardwareT::activate, &hardware ), "activate", hardware.get_name( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
if ( result )
{
// TODO( destogl ): make all command interfaces available ( currently are all available )
}
return result;
}
template <class HardwareT>
bool deactivate_hardware( HardwareT & hardware )
{
bool result = trigger_and_print_hardware_state_transition(
std::bind( &HardwareT::deactivate, &hardware ), "deactivate", hardware.get_name( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
if ( result )
{
// TODO( destogl ): make all command interfaces unavailable that should be present only
// when active ( currently are all available ) also at inactive
}
return result;
}
template <class HardwareT>
bool set_component_state( HardwareT & hardware, const rclcpp_lifecycle::State & target_state )
{
using lifecycle_msgs::msg::State;
bool result = false;
switch ( target_state.id( ) )
{
case State::PRIMARY_STATE_UNCONFIGURED:
switch ( hardware.get_state( ).id( ) )
{
case State::PRIMARY_STATE_UNCONFIGURED:
result = true;
break;
case State::PRIMARY_STATE_INACTIVE:
result = cleanup_hardware( hardware );
break;
case State::PRIMARY_STATE_ACTIVE:
result = deactivate_hardware( hardware );
if ( result )
{
result = cleanup_hardware( hardware );
}
break;
case State::PRIMARY_STATE_FINALIZED:
result = false;
RCUTILS_LOG_WARN_NAMED(
"resource_manager", "hardware '%s' is in finalized state and can be only destroyed.",
hardware.get_name( ).c_str( ) );
break;
}
break;
case State::PRIMARY_STATE_INACTIVE:
switch ( hardware.get_state( ).id( ) )
{
case State::PRIMARY_STATE_UNCONFIGURED:
result = configure_hardware( hardware );
break;
case State::PRIMARY_STATE_INACTIVE:
result = true;
break;
case State::PRIMARY_STATE_ACTIVE:
result = deactivate_hardware( hardware );
break;
case State::PRIMARY_STATE_FINALIZED:
result = false;
RCUTILS_LOG_WARN_NAMED(
"resource_manager", "hardware '%s' is in finalized state and can be only destroyed.",
hardware.get_name( ).c_str( ) );
break;
}
break;
case State::PRIMARY_STATE_ACTIVE:
switch ( hardware.get_state( ).id( ) )
{
case State::PRIMARY_STATE_UNCONFIGURED:
result = configure_hardware( hardware );
if ( result )
{
result = activate_hardware( hardware );
}
break;
case State::PRIMARY_STATE_INACTIVE:
result = activate_hardware( hardware );
break;
case State::PRIMARY_STATE_ACTIVE:
result = true;
break;
case State::PRIMARY_STATE_FINALIZED:
result = false;
RCUTILS_LOG_WARN_NAMED(
"resource_manager", "hardware '%s' is in finalized state and can be only destroyed.",
hardware.get_name( ).c_str( ) );
break;
}
break;
case State::PRIMARY_STATE_FINALIZED:
switch ( hardware.get_state( ).id( ) )
{
case State::PRIMARY_STATE_UNCONFIGURED:
result = shutdown_hardware( hardware );
break;
case State::PRIMARY_STATE_INACTIVE:
result = shutdown_hardware( hardware );
break;
case State::PRIMARY_STATE_ACTIVE:
result = shutdown_hardware( hardware );
break;
case State::PRIMARY_STATE_FINALIZED:
result = true;
break;
}
break;
}
return result;
}
template <class HardwareT>
void import_state_interfaces( HardwareT & hardware )
{
auto interfaces = hardware.export_state_interfaces( );
std::vector<std::string> interface_names;
interface_names.reserve( interfaces.size( ) );
for ( auto & interface : interfaces )
{
auto key = interface.get_name( );
state_interface_map_.emplace( std::make_pair( key, std::move( interface ) ) );
interface_names.push_back( key );
}
hardware_info_map_[hardware.get_name( )].state_interfaces = interface_names;
available_state_interfaces_.reserve(
available_state_interfaces_.capacity( ) + interface_names.size( ) );
}
template <class HardwareT>
void import_command_interfaces( HardwareT & hardware )
{
auto interfaces = hardware.export_command_interfaces( );
hardware_info_map_[hardware.get_name( )].command_interfaces = add_command_interfaces( interfaces );
}
/// Adds exported command interfaces into internal storage.
/**
* Add command interfaces to the internal storage. Command interfaces exported from hardware or
* chainable controllers are moved to the map with name-interface pairs, the interface names are
* added to the claimed map and available list's size is increased to reserve storage when
* interface change theirs status in real-time control loop.
*
* \param[interfaces] list of command interface to add into storage.
* \returns list of interface names that are added into internal storage. The output is used to
* avoid additional iterations to cache interface names, e.g., for initializing info structures.
*/
std::vector<std::string> add_command_interfaces( std::vector<CommandInterface> & interfaces )
{
std::vector<std::string> interface_names;
interface_names.reserve( interfaces.size( ) );
for ( auto & interface : interfaces )
{
auto key = interface.get_name( );
command_interface_map_.emplace( std::make_pair( key, std::move( interface ) ) );
claimed_command_interface_map_.emplace( std::make_pair( key, false ) );
interface_names.push_back( key );
}
available_command_interfaces_.reserve(
available_command_interfaces_.capacity( ) + interface_names.size( ) );
return interface_names;
}
/// Removes command interfaces from internal storage.
/**
* Command interface are removed from the maps with theirs storage and their claimed status.
*
* \param[interface_names] list of command interface names to remove from storage.
*/
void remove_command_interfaces( const std::vector<std::string> & interface_names )
{
for ( const auto & interface : interface_names )
{
command_interface_map_.erase( interface );
claimed_command_interface_map_.erase( interface );
}
}
void check_for_duplicates( const HardwareInfo & hardware_info )
{
// Check for identical names
if ( hardware_info_map_.find( hardware_info.name ) != hardware_info_map_.end( ) )
{
throw std::runtime_error(
"Hardware name " + hardware_info.name +
" is duplicated. Please provide a unique 'name' in the URDF." );
}
}
// TODO( destogl ): Propagate "false" up, if happens in initialize_hardware
void load_and_initialize_actuator( const HardwareInfo & hardware_info )
{
check_for_duplicates( hardware_info );
load_hardware<Actuator, ActuatorInterface>( hardware_info, actuator_loader_, actuators_ );
initialize_hardware( hardware_info, actuators_.back( ) );
import_state_interfaces( actuators_.back( ) );
import_command_interfaces( actuators_.back( ) );
}
void load_and_initialize_sensor( const HardwareInfo & hardware_info )
{
check_for_duplicates( hardware_info );
load_hardware<Sensor, SensorInterface>( hardware_info, sensor_loader_, sensors_ );
initialize_hardware( hardware_info, sensors_.back( ) );
import_state_interfaces( sensors_.back( ) );
}
void load_and_initialize_system( const HardwareInfo & hardware_info )
{
check_for_duplicates( hardware_info );
load_hardware<System, SystemInterface>( hardware_info, system_loader_, systems_ );
initialize_hardware( hardware_info, systems_.back( ) );
import_state_interfaces( systems_.back( ) );
import_command_interfaces( systems_.back( ) );
}
void initialize_actuator(
std::unique_ptr<ActuatorInterface> actuator, const HardwareInfo & hardware_info )
{
this->actuators_.emplace_back( Actuator( std::move( actuator ) ) );
initialize_hardware( hardware_info, actuators_.back( ) );
import_state_interfaces( actuators_.back( ) );
import_command_interfaces( actuators_.back( ) );
}
void initialize_sensor(
std::unique_ptr<SensorInterface> sensor, const HardwareInfo & hardware_info )
{
this->sensors_.emplace_back( Sensor( std::move( sensor ) ) );
initialize_hardware( hardware_info, sensors_.back( ) );
import_state_interfaces( sensors_.back( ) );
}
void initialize_system(
std::unique_ptr<SystemInterface> system, const HardwareInfo & hardware_info )
{
this->systems_.emplace_back( System( std::move( system ) ) );
initialize_hardware( hardware_info, systems_.back( ) );
import_state_interfaces( systems_.back( ) );
import_command_interfaces( systems_.back( ) );
}
// hardware plugins
pluginlib::ClassLoader<ActuatorInterface> actuator_loader_;
pluginlib::ClassLoader<SensorInterface> sensor_loader_;
pluginlib::ClassLoader<SystemInterface> system_loader_;
std::vector<Actuator> actuators_;
std::vector<Sensor> sensors_;
std::vector<System> systems_;
std::unordered_map<std::string, HardwareComponentInfo> hardware_info_map_;
std::unordered_map<std::string, std::vector<std::string>> controllers_reference_interfaces_map_;
/// Storage of all available state interfaces
std::map<std::string, StateInterface> state_interface_map_;
/// Storage of all available command interfaces
std::map<std::string, CommandInterface> command_interface_map_;
/// Vectors with interfaces available to controllers ( depending on hardware component state )
std::vector<std::string> available_state_interfaces_;
std::vector<std::string> available_command_interfaces_;
/// List of all claimed command interfaces
std::unordered_map<std::string, bool> claimed_command_interface_map_;
};
ResourceManager::ResourceManager( ) : resource_storage_( std::make_unique<ResourceStorage>( ) ) {}
ResourceManager::~ResourceManager( ) = default;
ResourceManager::ResourceManager(
const std::string & urdf, bool validate_interfaces, bool activate_all )
: resource_storage_( std::make_unique<ResourceStorage>( ) )
{
load_urdf( urdf, validate_interfaces );
if ( activate_all )
{
for ( auto const & hw_info : resource_storage_->hardware_info_map_ )
{
using lifecycle_msgs::msg::State;
rclcpp_lifecycle::State state( State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE );
set_component_state( hw_info.first, state );
}
}
}
void ResourceManager::load_urdf( const std::string & urdf, bool validate_interfaces )
{
const std::string system_type = "system";
const std::string sensor_type = "sensor";
const std::string actuator_type = "actuator";
const auto hardware_info = hardware_interface::parse_control_resources_from_urdf( urdf );
for ( const auto & individual_hardware_info : hardware_info )
{
if ( individual_hardware_info.type == actuator_type )
{
std::lock_guard<std::recursive_mutex> guard( resource_interfaces_lock_ );
std::lock_guard<std::recursive_mutex> guard_claimed( claimed_command_interfaces_lock_ );
resource_storage_->load_and_initialize_actuator( individual_hardware_info );
}
if ( individual_hardware_info.type == sensor_type )
{
std::lock_guard<std::recursive_mutex> guard( resource_interfaces_lock_ );
resource_storage_->load_and_initialize_sensor( individual_hardware_info );
}
if ( individual_hardware_info.type == system_type )
{
std::lock_guard<std::recursive_mutex> guard( resource_interfaces_lock_ );
std::lock_guard<std::recursive_mutex> guard_claimed( claimed_command_interfaces_lock_ );
resource_storage_->load_and_initialize_system( individual_hardware_info );
}
}
// throw on missing state and command interfaces, not specified keys are being ignored
if ( validate_interfaces )
{
validate_storage( hardware_info );
}
}
LoanedStateInterface ResourceManager::claim_state_interface( const std::string & key )
{
if ( !state_interface_is_available( key ) )
{
throw std::runtime_error( std::string( "State interface with key '" ) + key + "' does not exist" );
}
std::lock_guard<std::recursive_mutex> guard( resource_interfaces_lock_ );
return LoanedStateInterface( resource_storage_->state_interface_map_.at( key ) );
}
std::vector<std::string> ResourceManager::state_interface_keys( ) const
{
std::vector<std::string> keys;
std::lock_guard<std::recursive_mutex> guard( resource_interfaces_lock_ );
for ( const auto & item : resource_storage_->state_interface_map_ )
{
keys.push_back( std::get<0>( item ) );
}
return keys;
}
std::vector<std::string> ResourceManager::available_state_interfaces( ) const
{
std::lock_guard<std::recursive_mutex> guard( resource_interfaces_lock_ );
return resource_storage_->available_state_interfaces_;
}
bool ResourceManager::state_interface_exists( const std::string & key ) const
{
std::lock_guard<std::recursive_mutex> guard( resource_interfaces_lock_ );
return resource_storage_->state_interface_map_.find( key ) !=
resource_storage_->state_interface_map_.end( );
}
bool ResourceManager::state_interface_is_available( const std::string & name ) const
{
std::lock_guard<std::recursive_mutex> guard( resource_interfaces_lock_ );
return std::find(
resource_storage_->available_state_interfaces_.begin( ),
resource_storage_->available_state_interfaces_.end( ),
name ) != resource_storage_->available_state_interfaces_.end( );
}
void ResourceManager::import_controller_reference_interfaces(
const std::string & controller_name, std::vector<CommandInterface> & interfaces )
{
std::lock_guard<std::recursive_mutex> guard( resource_interfaces_lock_ );
std::lock_guard<std::recursive_mutex> guard_claimed( claimed_command_interfaces_lock_ );
auto interface_names = resource_storage_->add_command_interfaces( interfaces );
resource_storage_->controllers_reference_interfaces_map_[controller_name] = interface_names;
}
std::vector<std::string> ResourceManager::get_controller_reference_interface_names(
const std::string & controller_name )
{
return resource_storage_->controllers_reference_interfaces_map_.at( controller_name );
}
void ResourceManager::make_controller_reference_interfaces_available(
const std::string & controller_name )
{
auto interface_names =
resource_storage_->controllers_reference_interfaces_map_.at( controller_name );
std::lock_guard<std::recursive_mutex> guard( resource_interfaces_lock_ );
resource_storage_->available_command_interfaces_.insert(
resource_storage_->available_command_interfaces_.end( ), interface_names.begin( ),
interface_names.end( ) );
}
void ResourceManager::make_controller_reference_interfaces_unavailable(
const std::string & controller_name )
{
auto interface_names =
resource_storage_->controllers_reference_interfaces_map_.at( controller_name );
std::lock_guard<std::recursive_mutex> guard( resource_interfaces_lock_ );
for ( const auto & interface : interface_names )
{
auto found_it = std::find(
resource_storage_->available_command_interfaces_.begin( ),
resource_storage_->available_command_interfaces_.end( ), interface );
if ( found_it != resource_storage_->available_command_interfaces_.end( ) )
{
resource_storage_->available_command_interfaces_.erase( found_it );
RCUTILS_LOG_DEBUG_NAMED(
"resource_manager", "'%s' command interface removed from available list",
interface.c_str( ) );
}
}
}
void ResourceManager::remove_controller_reference_interfaces( const std::string & controller_name )
{
auto interface_names =
resource_storage_->controllers_reference_interfaces_map_.at( controller_name );
resource_storage_->controllers_reference_interfaces_map_.erase( controller_name );
std::lock_guard<std::recursive_mutex> guard( resource_interfaces_lock_ );
std::lock_guard<std::recursive_mutex> guard_claimed( claimed_command_interfaces_lock_ );
resource_storage_->remove_command_interfaces( interface_names );
}
// CM API: Called in "update"-thread
bool ResourceManager::command_interface_is_claimed( const std::string & key ) const
{
if ( !command_interface_is_available( key ) )
{
return false;
}
std::lock_guard<std::recursive_mutex> guard_claimed( claimed_command_interfaces_lock_ );
return resource_storage_->claimed_command_interface_map_.at( key );
}
// CM API: Called in "update"-thread
LoanedCommandInterface ResourceManager::claim_command_interface( const std::string & key )
{
if ( !command_interface_is_available( key ) )
{
throw std::runtime_error( std::string( "Command interface with '" ) + key + "' does not exist" );
}
std::lock_guard<std::recursive_mutex> guard_claimed( claimed_command_interfaces_lock_ );
if ( command_interface_is_claimed( key ) )
{
throw std::runtime_error(
std::string( "Command interface with '" ) + key + "' is already claimed" );
}
resource_storage_->claimed_command_interface_map_[key] = true;
std::lock_guard<std::recursive_mutex> guard( resource_interfaces_lock_ );
return LoanedCommandInterface(
resource_storage_->command_interface_map_.at( key ),
std::bind( &ResourceManager::release_command_interface, this, key ) );
}
// CM API: Called in "update"-thread
void ResourceManager::release_command_interface( const std::string & key )
{
std::lock_guard<std::recursive_mutex> guard_claimed( claimed_command_interfaces_lock_ );
resource_storage_->claimed_command_interface_map_[key] = false;
}
std::vector<std::string> ResourceManager::command_interface_keys( ) const
{
std::vector<std::string> keys;
std::lock_guard<std::recursive_mutex> guard( resource_interfaces_lock_ );
for ( const auto & item : resource_storage_->command_interface_map_ )
{
keys.push_back( std::get<0>( item ) );
}
return keys;
}
std::vector<std::string> ResourceManager::available_command_interfaces( ) const
{
std::lock_guard<std::recursive_mutex> guard( resource_interfaces_lock_ );
return resource_storage_->available_command_interfaces_;
}
bool ResourceManager::command_interface_exists( const std::string & key ) const
{
std::lock_guard<std::recursive_mutex> guard( resource_interfaces_lock_ );
return resource_storage_->command_interface_map_.find( key ) !=
resource_storage_->command_interface_map_.end( );
}
// CM API
bool ResourceManager::command_interface_is_available( const std::string & name ) const
{
std::lock_guard<std::recursive_mutex> guard( resource_interfaces_lock_ );
return std::find(
resource_storage_->available_command_interfaces_.begin( ),
resource_storage_->available_command_interfaces_.end( ),
name ) != resource_storage_->available_command_interfaces_.end( );
}
size_t ResourceManager::actuator_components_size( ) const
{
return resource_storage_->actuators_.size( );
}
size_t ResourceManager::sensor_components_size( ) const
{
return resource_storage_->sensors_.size( );
}
void ResourceManager::import_component(
std::unique_ptr<ActuatorInterface> actuator, const HardwareInfo & hardware_info )
{
resource_storage_->initialize_actuator( std::move( actuator ), hardware_info );
}
void ResourceManager::import_component(
std::unique_ptr<SensorInterface> sensor, const HardwareInfo & hardware_info )
{
resource_storage_->initialize_sensor( std::move( sensor ), hardware_info );
}
void ResourceManager::import_component(
std::unique_ptr<SystemInterface> system, const HardwareInfo & hardware_info )
{
resource_storage_->initialize_system( std::move( system ), hardware_info );
}
size_t ResourceManager::system_components_size( ) const
{
return resource_storage_->systems_.size( );
}
// End of "used only in tests"
std::unordered_map<std::string, HardwareComponentInfo> ResourceManager::get_components_status( )
{
for ( auto & component : resource_storage_->actuators_ )
{
resource_storage_->hardware_info_map_[component.get_name( )].state = component.get_state( );
}
for ( auto & component : resource_storage_->sensors_ )
{
resource_storage_->hardware_info_map_[component.get_name( )].state = component.get_state( );
}
for ( auto & component : resource_storage_->systems_ )
{
resource_storage_->hardware_info_map_[component.get_name( )].state = component.get_state( );
}
return resource_storage_->hardware_info_map_;
}
bool ResourceManager::prepare_command_mode_switch(
const std::vector<std::string> & start_interfaces,
const std::vector<std::string> & stop_interfaces )
{
auto interfaces_to_string = [&]( )
{
std::stringstream ss;
ss << "Start interfaces: " << std::endl << "[" << std::endl;
for ( const auto & start_if : start_interfaces )
{
ss << " " << start_if << std::endl;
}
ss << "]" << std::endl;
ss << "Stop interfaces: " << std::endl << "[" << std::endl;
for ( const auto & stop_if : stop_interfaces )
{
ss << " " << stop_if << std::endl;
}
ss << "]" << std::endl;
return ss.str( );
};
for ( auto & component : resource_storage_->actuators_ )
{
if ( return_type::OK != component.prepare_command_mode_switch( start_interfaces, stop_interfaces ) )
{
RCUTILS_LOG_ERROR_NAMED(
"resource_manager", "Component '%s' did not accept new command resource combination: \n %s",
component.get_name( ).c_str( ), interfaces_to_string( ).c_str( ) );
return false;
}
}
for ( auto & component : resource_storage_->systems_ )
{
if ( return_type::OK != component.prepare_command_mode_switch( start_interfaces, stop_interfaces ) )
{
RCUTILS_LOG_ERROR_NAMED(
"resource_manager", "Component '%s' did not accept new command resource combination: \n %s",
component.get_name( ).c_str( ), interfaces_to_string( ).c_str( ) );
return false;
}
}
return true;
}
bool ResourceManager::perform_command_mode_switch(
const std::vector<std::string> & start_interfaces,
const std::vector<std::string> & stop_interfaces )
{
for ( auto & component : resource_storage_->actuators_ )
{
if ( return_type::OK != component.perform_command_mode_switch( start_interfaces, stop_interfaces ) )
{
RCUTILS_LOG_ERROR_NAMED(
"resource_manager", "Component '%s' could not perform switch",
component.get_name( ).c_str( ) );
return false;
}
}
for ( auto & component : resource_storage_->systems_ )
{
if ( return_type::OK != component.perform_command_mode_switch( start_interfaces, stop_interfaces ) )
{
RCUTILS_LOG_ERROR_NAMED(
"resource_manager", "Component '%s' could not perform switch",
component.get_name( ).c_str( ) );
return false;
}
}
return true;
}
return_type ResourceManager::set_component_state(
const std::string & component_name, rclcpp_lifecycle::State & target_state )
{
using lifecycle_msgs::msg::State;
using std::placeholders::_1;
using std::placeholders::_2;
auto found_it = resource_storage_->hardware_info_map_.find( component_name );
if ( found_it == resource_storage_->hardware_info_map_.end( ) )
{
RCUTILS_LOG_INFO_NAMED(
"resource_manager", "Hardware Component with name '%s' does not exists",
component_name.c_str( ) );
return return_type::ERROR;
}
return_type result = return_type::OK;
if ( target_state.id( ) == 0 )
{
if ( target_state.label( ) == lifecycle_state_names::UNCONFIGURED )
{
target_state = rclcpp_lifecycle::State(
State::PRIMARY_STATE_UNCONFIGURED, lifecycle_state_names::UNCONFIGURED );
}
if ( target_state.label( ) == lifecycle_state_names::INACTIVE )
{
target_state =
rclcpp_lifecycle::State( State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE );
}
if ( target_state.label( ) == lifecycle_state_names::ACTIVE )
{
target_state =
rclcpp_lifecycle::State( State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE );
}
if ( target_state.label( ) == lifecycle_state_names::FINALIZED )
{
target_state =
rclcpp_lifecycle::State( State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED );
}
}
auto find_set_component_state = [&]( auto action, auto & components )
{
auto found_component_it = std::find_if(
components.begin( ), components.end( ),
[&]( const auto & component ) { return component.get_name( ) == component_name; } );
if ( found_component_it != components.end( ) )
{
if ( action( *found_component_it, target_state ) )
{
result = return_type::OK;
}
else
{
result = return_type::ERROR;
}
return true;
}
return false;
};
bool found = find_set_component_state(
std::bind( &ResourceStorage::set_component_state<Actuator>, resource_storage_.get( ), _1, _2 ),
resource_storage_->actuators_ );
if ( !found )
{
found = find_set_component_state(
std::bind( &ResourceStorage::set_component_state<Sensor>, resource_storage_.get( ), _1, _2 ),
resource_storage_->sensors_ );
}
if ( !found )
{
found = find_set_component_state(
std::bind( &ResourceStorage::set_component_state<System>, resource_storage_.get( ), _1, _2 ),
resource_storage_->systems_ );
}
return result;
}
void ResourceManager::read( const rclcpp::Time & time, const rclcpp::Duration & period )
{
for ( auto & component : resource_storage_->actuators_ )
{
component.read( time, period );
}
for ( auto & component : resource_storage_->sensors_ )
{
component.read( time, period );
}
for ( auto & component : resource_storage_->systems_ )
{
component.read( time, period );
}
}
void ResourceManager::write( const rclcpp::Time & time, const rclcpp::Duration & period )
{
for ( auto & component : resource_storage_->actuators_ )
{
component.write( time, period );
}
for ( auto & component : resource_storage_->systems_ )
{
component.write( time, period );
}
}
void ResourceManager::validate_storage(
const std::vector<hardware_interface::HardwareInfo> & hardware_info ) const
{
std::vector<std::string> missing_state_keys = {};
std::vector<std::string> missing_command_keys = {};
for ( const auto & hardware : hardware_info )
{
for ( const auto & joint : hardware.joints )
{
for ( const auto & state_interface : joint.state_interfaces )
{
if ( !state_interface_exists( joint.name + "/" + state_interface.name ) )
{
missing_state_keys.emplace_back( joint.name + "/" + state_interface.name );
}
}
for ( const auto & command_interface : joint.command_interfaces )
{
if ( !command_interface_exists( joint.name + "/" + command_interface.name ) )
{
missing_command_keys.emplace_back( joint.name + "/" + command_interface.name );
}
}
}
for ( const auto & sensor : hardware.sensors )
{
for ( const auto & state_interface : sensor.state_interfaces )
{
if ( !state_interface_exists( sensor.name + "/" + state_interface.name ) )
{
missing_state_keys.emplace_back( sensor.name + "/" + state_interface.name );
}
}
}
}
if ( !missing_state_keys.empty( ) || !missing_command_keys.empty( ) )
{
std::string err_msg = "Wrong state or command interface configuration.\n";
err_msg += "missing state interfaces:\n";
for ( const auto & missing_key : missing_state_keys )
{
err_msg += "' " + missing_key + " '" + "\t";
}
err_msg += "\nmissing command interfaces:\n";
for ( const auto & missing_key : missing_command_keys )
{
err_msg += "' " + missing_key + " '" + "\t";
}
throw std::runtime_error( err_msg );
}
}
// Temporary method to keep old interface and reduce framework changes in PRs
void ResourceManager::activate_all_components( )
{
using lifecycle_msgs::msg::State;
rclcpp_lifecycle::State active_state(
State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE );
for ( auto & component : resource_storage_->actuators_ )
{
set_component_state( component.get_name( ), active_state );
}
for ( auto & component : resource_storage_->sensors_ )
{
set_component_state( component.get_name( ), active_state );
}
for ( auto & component : resource_storage_->systems_ )
{
set_component_state( component.get_name( ), active_state );
}
}
} // namespace hardware_interface
1 // Copyright 2020 - 2021 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "hardware_interface/sensor.hpp"
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/sensor_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
namespace hardware_interface
{
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
35 Sensor::Sensor( std::unique_ptr<SensorInterface> impl ) : impl_( std::move( impl ) ) {}
37 const rclcpp_lifecycle::State & Sensor::initialize( const HardwareInfo & sensor_info )
{
if ( impl_->get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN )
{
switch ( impl_->on_init( sensor_info ) )
{
case CallbackReturn::SUCCESS:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
lifecycle_state_names::UNCONFIGURED ) );
break;
case CallbackReturn::FAILURE:
case CallbackReturn::ERROR:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED ) );
break;
}
}
return impl_->get_state( );
}
58 const rclcpp_lifecycle::State & Sensor::configure( )
{
if ( impl_->get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED )
{
switch ( impl_->on_configure( impl_->get_state( ) ) )
{
case CallbackReturn::SUCCESS:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE ) );
break;
case CallbackReturn::FAILURE:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
lifecycle_state_names::UNCONFIGURED ) );
break;
case CallbackReturn::ERROR:
impl_->set_state( error( ) );
break;
}
}
return impl_->get_state( );
}
81 const rclcpp_lifecycle::State & Sensor::cleanup( )
{
if ( impl_->get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE )
{
switch ( impl_->on_cleanup( impl_->get_state( ) ) )
{
case CallbackReturn::SUCCESS:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
lifecycle_state_names::UNCONFIGURED ) );
break;
case CallbackReturn::FAILURE:
case CallbackReturn::ERROR:
impl_->set_state( error( ) );
break;
}
}
return impl_->get_state( );
}
101 const rclcpp_lifecycle::State & Sensor::shutdown( )
{
if (
impl_->get_state( ).id( ) != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN &&
impl_->get_state( ).id( ) != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED )
{
switch ( impl_->on_shutdown( impl_->get_state( ) ) )
{
case CallbackReturn::SUCCESS:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED ) );
break;
case CallbackReturn::FAILURE:
case CallbackReturn::ERROR:
impl_->set_state( error( ) );
break;
}
}
return impl_->get_state( );
}
122 const rclcpp_lifecycle::State & Sensor::activate( )
{
if ( impl_->get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE )
{
switch ( impl_->on_activate( impl_->get_state( ) ) )
{
case CallbackReturn::SUCCESS:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE ) );
break;
case CallbackReturn::FAILURE:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE ) );
break;
case CallbackReturn::ERROR:
impl_->set_state( error( ) );
break;
}
}
return impl_->get_state( );
}
144 const rclcpp_lifecycle::State & Sensor::deactivate( )
{
if ( impl_->get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
{
switch ( impl_->on_deactivate( impl_->get_state( ) ) )
{
case CallbackReturn::SUCCESS:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE ) );
break;
case CallbackReturn::FAILURE:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE ) );
break;
case CallbackReturn::ERROR:
impl_->set_state( error( ) );
break;
}
}
return impl_->get_state( );
}
166 const rclcpp_lifecycle::State & Sensor::error( )
{
if ( impl_->get_state( ).id( ) != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN )
{
switch ( impl_->on_error( impl_->get_state( ) ) )
{
case CallbackReturn::SUCCESS:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
lifecycle_state_names::UNCONFIGURED ) );
break;
case CallbackReturn::FAILURE:
case CallbackReturn::ERROR:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED ) );
break;
}
}
return impl_->get_state( );
}
187 std::vector<StateInterface> Sensor::export_state_interfaces( )
{
return impl_->export_state_interfaces( );
}
192 std::string Sensor::get_name( ) const { return impl_->get_name( ); }
194 const rclcpp_lifecycle::State & Sensor::get_state( ) const { return impl_->get_state( ); }
196 return_type Sensor::read( const rclcpp::Time & time, const rclcpp::Duration & period )
{
return_type result = return_type::ERROR;
if (
impl_->get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
impl_->get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
{
result = impl_->read( time, period );
if ( result == return_type::ERROR )
{
error( );
}
}
return result;
}
} // namespace hardware_interface
1 // Copyright 2020 - 2021 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "hardware_interface/system.hpp"
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
namespace hardware_interface
{
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
35 System::System( std::unique_ptr<SystemInterface> impl ) : impl_( std::move( impl ) ) {}
37 const rclcpp_lifecycle::State & System::initialize( const HardwareInfo & system_info )
{
if ( impl_->get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN )
{
switch ( impl_->on_init( system_info ) )
{
case CallbackReturn::SUCCESS:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
lifecycle_state_names::UNCONFIGURED ) );
break;
case CallbackReturn::FAILURE:
case CallbackReturn::ERROR:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED ) );
break;
}
}
return impl_->get_state( );
}
58 const rclcpp_lifecycle::State & System::configure( )
{
if ( impl_->get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED )
{
switch ( impl_->on_configure( impl_->get_state( ) ) )
{
case CallbackReturn::SUCCESS:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE ) );
break;
case CallbackReturn::FAILURE:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
lifecycle_state_names::UNCONFIGURED ) );
break;
case CallbackReturn::ERROR:
impl_->set_state( error( ) );
break;
}
}
return impl_->get_state( );
}
81 const rclcpp_lifecycle::State & System::cleanup( )
{
if ( impl_->get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE )
{
switch ( impl_->on_cleanup( impl_->get_state( ) ) )
{
case CallbackReturn::SUCCESS:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
lifecycle_state_names::UNCONFIGURED ) );
break;
case CallbackReturn::FAILURE:
case CallbackReturn::ERROR:
impl_->set_state( error( ) );
break;
}
}
return impl_->get_state( );
}
101 const rclcpp_lifecycle::State & System::shutdown( )
{
if (
impl_->get_state( ).id( ) != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN &&
impl_->get_state( ).id( ) != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED )
{
switch ( impl_->on_shutdown( impl_->get_state( ) ) )
{
case CallbackReturn::SUCCESS:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED ) );
break;
case CallbackReturn::FAILURE:
case CallbackReturn::ERROR:
impl_->set_state( error( ) );
break;
}
}
return impl_->get_state( );
}
122 const rclcpp_lifecycle::State & System::activate( )
{
if ( impl_->get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE )
{
switch ( impl_->on_activate( impl_->get_state( ) ) )
{
case CallbackReturn::SUCCESS:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE ) );
break;
case CallbackReturn::FAILURE:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE ) );
break;
case CallbackReturn::ERROR:
impl_->set_state( error( ) );
break;
}
}
return impl_->get_state( );
}
144 const rclcpp_lifecycle::State & System::deactivate( )
{
if ( impl_->get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
{
switch ( impl_->on_deactivate( impl_->get_state( ) ) )
{
case CallbackReturn::SUCCESS:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE ) );
break;
case CallbackReturn::FAILURE:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE ) );
break;
case CallbackReturn::ERROR:
impl_->set_state( error( ) );
break;
}
}
return impl_->get_state( );
}
166 const rclcpp_lifecycle::State & System::error( )
{
if ( impl_->get_state( ).id( ) != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN )
{
switch ( impl_->on_error( impl_->get_state( ) ) )
{
case CallbackReturn::SUCCESS:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
lifecycle_state_names::UNCONFIGURED ) );
break;
case CallbackReturn::FAILURE:
case CallbackReturn::ERROR:
impl_->set_state( rclcpp_lifecycle::State(
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED ) );
break;
}
}
return impl_->get_state( );
}
187 std::vector<StateInterface> System::export_state_interfaces( )
{
return impl_->export_state_interfaces( );
}
192 std::vector<CommandInterface> System::export_command_interfaces( )
{
return impl_->export_command_interfaces( );
}
197 return_type System::prepare_command_mode_switch(
198 const std::vector<std::string> & start_interfaces,
199 const std::vector<std::string> & stop_interfaces )
{
return impl_->prepare_command_mode_switch( start_interfaces, stop_interfaces );
}
204 return_type System::perform_command_mode_switch(
205 const std::vector<std::string> & start_interfaces,
206 const std::vector<std::string> & stop_interfaces )
{
return impl_->perform_command_mode_switch( start_interfaces, stop_interfaces );
}
211 std::string System::get_name( ) const { return impl_->get_name( ); }
213 const rclcpp_lifecycle::State & System::get_state( ) const { return impl_->get_state( ); }
215 return_type System::read( const rclcpp::Time & time, const rclcpp::Duration & period )
{
return_type result = return_type::ERROR;
if (
impl_->get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
impl_->get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
{
result = impl_->read( time, period );
if ( result == return_type::ERROR )
{
error( );
}
}
return result;
}
231 return_type System::write( const rclcpp::Time & time, const rclcpp::Duration & period )
{
return_type result = return_type::ERROR;
if (
impl_->get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
impl_->get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE )
{
result = impl_->write( time, period );
if ( result == return_type::ERROR )
{
error( );
}
}
return result;
}
} // namespace hardware_interface
// Copyright ( c ) 2021 PickNik, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Author: Denis Stogl
#include <gmock/gmock.h>
#include <cmath>
#include <string>
#include <unordered_map>
#include <vector>
#include "hardware_interface/loaned_command_interface.hpp"
#include "hardware_interface/loaned_state_interface.hpp"
#include "hardware_interface/resource_manager.hpp"
#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "ros2_control_test_assets/components_urdfs.hpp"
#include "ros2_control_test_assets/descriptions.hpp"
namespace
{
35 const auto TIME = rclcpp::Time( 0 );
const auto PERIOD = rclcpp::Duration::from_seconds( 0.01 );
} // namespace
class TestGenericSystem : public ::testing::Test
{
protected:
void SetUp( ) override
{
// REMOVE THIS MEMBER ONCE FAKE COMPONENTS ARE REMOVED
hardware_fake_system_2dof_ =
R"(
<ros2_control name="GenericSystem2dof" type="system">
<hardware>
<plugin>fake_components/GenericSystem</plugin>
</hardware>
<joint name="joint1">
<command_interface name="position"/>
<state_interface name="position"/>
<param name="initial_position">1.57</param>
</joint>
<joint name="joint2">
<command_interface name="position"/>
<state_interface name="position"/>
<param name="initial_position">0.7854</param>
</joint>
</ros2_control>
)";
hardware_system_2dof_ =
R"(
<ros2_control name="GenericSystem2dof" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
</hardware>
<joint name="joint1">
<command_interface name="position"/>
<state_interface name="position"/>
<param name="initial_position">1.57</param>
</joint>
<joint name="joint2">
<command_interface name="position"/>
<state_interface name="position"/>
<param name="initial_position">0.7854</param>
</joint>
</ros2_control>
)";
hardware_system_2dof_asymetric_ =
R"(
<ros2_control name="GenericSystem2dof" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
</hardware>
<joint name="joint1">
<command_interface name="position"/>
<state_interface name="velocity"/>
<param name="initial_position">1.57</param>
</joint>
<joint name="joint2">
<command_interface name="acceleration"/>
<state_interface name="position"/>
<param name="initial_position">0.7854</param>
<param name="initial_acceleration">0.8554</param>
</joint>
</ros2_control>
)";
hardware_system_2dof_standard_interfaces_ =
R"(
<ros2_control name="GenericSystem2dof" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
</hardware>
<joint name="joint1">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<param name="initial_position">3.45</param>
</joint>
<joint name="joint2">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<param name="initial_position">2.78</param>
</joint>
</ros2_control>
)";
hardware_system_2dof_with_other_interface_ =
R"(
<ros2_control name="GenericSystem2dof" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
</hardware>
<joint name="joint1">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<param name="initial_position">1.55</param>
<param name="initial_velocity">0.1</param>
</joint>
<joint name="joint2">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<param name="initial_position">0.65</param>
<param name="initial_velocity">0.2</param>
</joint>
<joint name="voltage_output">
<command_interface name="voltage"/>
<state_interface name="voltage"/>
<param name="initial_voltage">0.5</param>
</joint>
</ros2_control>
)";
hardware_system_2dof_with_sensor_ =
R"(
<ros2_control name="GenericSystem2dof" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
</hardware>
<joint name="joint1">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="joint2">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<sensor name="tcp_force_sensor">
<state_interface name="fx"/>
<state_interface name="fy"/>
<state_interface name="tx"/>
<state_interface name="ty"/>
<param name="frame_id">kuka_tcp</param>
</sensor>
</ros2_control>
)";
hardware_system_2dof_with_sensor_fake_command_ =
R"(
<ros2_control name="GenericSystem2dof" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
<param name="fake_sensor_commands">true</param>
</hardware>
<joint name="joint1">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="joint2">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<sensor name="tcp_force_sensor">
<state_interface name="fx"/>
<state_interface name="fy"/>
<state_interface name="tx"/>
<state_interface name="ty"/>
<param name="frame_id">kuka_tcp</param>
</sensor>
</ros2_control>
)";
hardware_system_2dof_with_sensor_fake_command_True_ =
R"(
<ros2_control name="GenericSystem2dof" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
<param name="fake_sensor_commands">True</param>
</hardware>
<joint name="joint1">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="joint2">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<sensor name="tcp_force_sensor">
<state_interface name="fx"/>
<state_interface name="fy"/>
<state_interface name="tx"/>
<state_interface name="ty"/>
<param name="frame_id">kuka_tcp</param>
</sensor>
</ros2_control>
)";
hardware_system_2dof_with_mimic_joint_ =
R"(
<ros2_control name="GenericSystem2dof" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
</hardware>
<joint name="joint1">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<param name="initial_position">1.57</param>
</joint>
<joint name="joint2">
<param name="mimic">joint1</param>
<param name="multiplier">-2</param>
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
</ros2_control>
)";
hardware_system_2dof_standard_interfaces_with_offset_ =
R"(
<ros2_control name="GenericSystem2dof" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
<param name="position_state_following_offset">-3</param>
</hardware>
<joint name="joint1">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position">
<param name="initial_value">3.45</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="joint2">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position">
<param name="initial_value">2.78</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
</ros2_control>
)";
hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_missing_ =
R"(
<ros2_control name="GenericSystem2dof" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
<param name="position_state_following_offset">-3</param>
<param name="custom_interface_with_following_offset">actual_position</param>
</hardware>
<joint name="joint1">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<param name="initial_position">3.45</param>
</joint>
<joint name="joint2">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<param name="initial_position">2.78</param>
</joint>
</ros2_control>
)";
hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_ =
R"(
<ros2_control name="GenericSystem2dof" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
<param name="position_state_following_offset">-3</param>
<param name="custom_interface_with_following_offset">actual_position</param>
</hardware>
<joint name="joint1">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="actual_position"/>
<param name="initial_position">3.45</param>
</joint>
<joint name="joint2">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="actual_position"/>
<param name="initial_position">2.78</param>
</joint>
</ros2_control>
)";
valid_urdf_ros2_control_system_robot_with_gpio_ =
R"(
<ros2_control name="GenericSystem2dof" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
<param name="example_param_write_for_sec">2</param>
<param name="example_param_read_for_sec">2</param>
</hardware>
<joint name="joint1">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position">
<param name="initial_value">3.45</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="joint2">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<param name="initial_position">2.78</param>
</joint>
<gpio name="flange_analog_IOs">
<command_interface name="analog_output1" data_type="double"/>
<state_interface name="analog_output1"/>
<state_interface name="analog_input1"/>
<state_interface name="analog_input2"/>
</gpio>
<gpio name="flange_vacuum">
<command_interface name="vacuum"/>
<state_interface name="vacuum" data_type="double"/>
</gpio>
</ros2_control>
)";
valid_urdf_ros2_control_system_robot_with_gpio_fake_command_ =
R"(
<ros2_control name="GenericSystem2dof" type="system">
<hardware>
<plugin>mock_components/GenericSystem</plugin>
<param name="fake_gpio_commands">True</param>
</hardware>
<joint name="joint1">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<param name="initial_position">3.45</param>
</joint>
<joint name="joint2">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<param name="initial_position">2.78</param>
</joint>
<gpio name="flange_analog_IOs">
<command_interface name="analog_output1" data_type="double"/>
<state_interface name="analog_output1"/>
<state_interface name="analog_input1"/>
<state_interface name="analog_input2"/>
</gpio>
<gpio name="flange_vacuum">
<command_interface name="vacuum"/>
<state_interface name="vacuum" data_type="double"/>
</gpio>
</ros2_control>
)";
}
std::string hardware_robot_2dof_;
std::string hardware_system_2dof_;
std::string hardware_fake_system_2dof_;
std::string hardware_system_2dof_asymetric_;
std::string hardware_system_2dof_standard_interfaces_;
std::string hardware_system_2dof_with_other_interface_;
std::string hardware_system_2dof_with_sensor_;
std::string hardware_system_2dof_with_sensor_fake_command_;
std::string hardware_system_2dof_with_sensor_fake_command_True_;
std::string hardware_system_2dof_with_mimic_joint_;
std::string hardware_system_2dof_standard_interfaces_with_offset_;
std::string hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_;
std::string hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_missing_;
std::string valid_urdf_ros2_control_system_robot_with_gpio_;
std::string valid_urdf_ros2_control_system_robot_with_gpio_fake_command_;
};
void set_components_state(
hardware_interface::ResourceManager & rm, const std::vector<std::string> & components,
const uint8_t state_id, const std::string & state_name )
{
for ( const auto & component : components )
{
rclcpp_lifecycle::State state( state_id, state_name );
rm.set_component_state( component, state );
}
}
auto configure_components = [](
hardware_interface::ResourceManager & rm,
const std::vector<std::string> & components = {"GenericSystem2dof"} )
{
set_components_state(
rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
hardware_interface::lifecycle_state_names::INACTIVE );
};
auto activate_components = [](
hardware_interface::ResourceManager & rm,
const std::vector<std::string> & components = {"GenericSystem2dof"} )
{
set_components_state(
rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
hardware_interface::lifecycle_state_names::ACTIVE );
};
auto deactivate_components = [](
hardware_interface::ResourceManager & rm,
const std::vector<std::string> & components = {"GenericSystem2dof"} )
{
set_components_state(
rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
hardware_interface::lifecycle_state_names::INACTIVE );
};
TEST_F( TestGenericSystem, load_generic_system_2dof )
{
auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_ +
ros2_control_test_assets::urdf_tail;
ASSERT_NO_THROW( hardware_interface::ResourceManager rm( urdf ) );
}
// REMOVE THIS TEST ONCE FAKE COMPONENTS ARE REMOVED
TEST_F( TestGenericSystem, generic_fake_system_2dof_symetric_interfaces )
{
auto urdf = ros2_control_test_assets::urdf_head + hardware_fake_system_2dof_ +
ros2_control_test_assets::urdf_tail;
hardware_interface::ResourceManager rm( urdf );
// Activate components to get all interfaces available
activate_components( rm );
// Check interfaces
EXPECT_EQ( 1u, rm.system_components_size( ) );
ASSERT_EQ( 2u, rm.state_interface_keys( ).size( ) );
EXPECT_TRUE( rm.state_interface_exists( "joint1/position" ) );
EXPECT_TRUE( rm.state_interface_exists( "joint2/position" ) );
ASSERT_EQ( 2u, rm.command_interface_keys( ).size( ) );
EXPECT_TRUE( rm.command_interface_exists( "joint1/position" ) );
EXPECT_TRUE( rm.command_interface_exists( "joint2/position" ) );
// Check initial values
hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface( "joint1/position" );
hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface( "joint2/position" );
hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface( "joint1/position" );
hardware_interface::LoanedCommandInterface j2p_c = rm.claim_command_interface( "joint2/position" );
ASSERT_EQ( 1.57, j1p_s.get_value( ) );
ASSERT_EQ( 0.7854, j2p_s.get_value( ) );
ASSERT_TRUE( std::isnan( j1p_c.get_value( ) ) );
ASSERT_TRUE( std::isnan( j2p_c.get_value( ) ) );
}
// Test inspired by hardware_interface/test_resource_manager.cpp
TEST_F( TestGenericSystem, generic_system_2dof_symetric_interfaces )
{
auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_ +
ros2_control_test_assets::urdf_tail;
hardware_interface::ResourceManager rm( urdf );
// Activate components to get all interfaces available
activate_components( rm );
// Check interfaces
EXPECT_EQ( 1u, rm.system_components_size( ) );
ASSERT_EQ( 2u, rm.state_interface_keys( ).size( ) );
EXPECT_TRUE( rm.state_interface_exists( "joint1/position" ) );
EXPECT_TRUE( rm.state_interface_exists( "joint2/position" ) );
ASSERT_EQ( 2u, rm.command_interface_keys( ).size( ) );
EXPECT_TRUE( rm.command_interface_exists( "joint1/position" ) );
EXPECT_TRUE( rm.command_interface_exists( "joint2/position" ) );
// Check initial values
hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface( "joint1/position" );
hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface( "joint2/position" );
hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface( "joint1/position" );
hardware_interface::LoanedCommandInterface j2p_c = rm.claim_command_interface( "joint2/position" );
ASSERT_EQ( 1.57, j1p_s.get_value( ) );
ASSERT_EQ( 0.7854, j2p_s.get_value( ) );
ASSERT_TRUE( std::isnan( j1p_c.get_value( ) ) );
ASSERT_TRUE( std::isnan( j2p_c.get_value( ) ) );
}
// Test inspired by hardware_interface/test_resource_manager.cpp
TEST_F( TestGenericSystem, generic_system_2dof_asymetric_interfaces )
{
auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_asymetric_ +
ros2_control_test_assets::urdf_tail;
hardware_interface::ResourceManager rm( urdf );
// Activate components to get all interfaces available
activate_components( rm );
// Check interfaces
EXPECT_EQ( 1u, rm.system_components_size( ) );
ASSERT_EQ( 2u, rm.state_interface_keys( ).size( ) );
EXPECT_FALSE( rm.state_interface_exists( "joint1/position" ) );
EXPECT_TRUE( rm.state_interface_exists( "joint1/velocity" ) );
EXPECT_FALSE( rm.state_interface_exists( "joint1/acceleration" ) );
EXPECT_TRUE( rm.state_interface_exists( "joint2/position" ) );
EXPECT_FALSE( rm.state_interface_exists( "joint2/velocity" ) );
EXPECT_FALSE( rm.state_interface_exists( "joint2/acceleration" ) );
ASSERT_EQ( 2u, rm.command_interface_keys( ).size( ) );
EXPECT_TRUE( rm.command_interface_exists( "joint1/position" ) );
EXPECT_FALSE( rm.command_interface_exists( "joint1/velocity" ) );
EXPECT_FALSE( rm.command_interface_exists( "joint1/acceleration" ) );
EXPECT_FALSE( rm.command_interface_exists( "joint2/position" ) );
EXPECT_FALSE( rm.command_interface_exists( "joint2/velocity" ) );
EXPECT_TRUE( rm.command_interface_exists( "joint2/acceleration" ) );
// Check initial values
ASSERT_ANY_THROW( rm.claim_state_interface( "joint1/position" ) );
hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface( "joint1/velocity" );
ASSERT_ANY_THROW( rm.claim_state_interface( "joint1/acceleration" ) );
hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface( "joint2/position" );
ASSERT_ANY_THROW( rm.claim_state_interface( "joint2/velocity" ) );
ASSERT_ANY_THROW( rm.claim_state_interface( "joint2/acceleration" ) );
hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface( "joint1/position" );
ASSERT_ANY_THROW( rm.claim_command_interface( "joint1/velocity" ) );
ASSERT_ANY_THROW( rm.claim_command_interface( "joint1/acceleration" ) );
ASSERT_ANY_THROW( rm.claim_command_interface( "joint2/position" ) );
ASSERT_ANY_THROW( rm.claim_command_interface( "joint2/position" ) );
hardware_interface::LoanedCommandInterface j2a_c =
rm.claim_command_interface( "joint2/acceleration" );
ASSERT_EQ( 0.0, j1v_s.get_value( ) );
ASSERT_EQ( 0.7854, j2p_s.get_value( ) );
ASSERT_TRUE( std::isnan( j1p_c.get_value( ) ) );
ASSERT_TRUE( std::isnan( j2a_c.get_value( ) ) );
}
void generic_system_functional_test( const std::string & urdf, const double offset = 0 )
{
hardware_interface::ResourceManager rm( urdf );
// check is hardware is configured
auto status_map = rm.get_components_status( );
EXPECT_EQ(
status_map["GenericSystem2dof"].state.label( ),
hardware_interface::lifecycle_state_names::UNCONFIGURED );
configure_components( rm );
status_map = rm.get_components_status( );
EXPECT_EQ(
status_map["GenericSystem2dof"].state.label( ),
hardware_interface::lifecycle_state_names::INACTIVE );
activate_components( rm );
status_map = rm.get_components_status( );
EXPECT_EQ(
status_map["GenericSystem2dof"].state.label( ),
hardware_interface::lifecycle_state_names::ACTIVE );
// Check initial values
hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface( "joint1/position" );
hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface( "joint1/velocity" );
hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface( "joint2/position" );
hardware_interface::LoanedStateInterface j2v_s = rm.claim_state_interface( "joint2/velocity" );
hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface( "joint1/position" );
hardware_interface::LoanedCommandInterface j1v_c = rm.claim_command_interface( "joint1/velocity" );
hardware_interface::LoanedCommandInterface j2p_c = rm.claim_command_interface( "joint2/position" );
hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface( "joint2/velocity" );
// State interfaces without initial value are set to 0
ASSERT_EQ( 3.45, j1p_s.get_value( ) );
ASSERT_EQ( 0.0, j1v_s.get_value( ) );
ASSERT_EQ( 2.78, j2p_s.get_value( ) );
ASSERT_EQ( 0.0, j2v_s.get_value( ) );
ASSERT_TRUE( std::isnan( j1p_c.get_value( ) ) );
ASSERT_TRUE( std::isnan( j1v_c.get_value( ) ) );
ASSERT_TRUE( std::isnan( j2p_c.get_value( ) ) );
ASSERT_TRUE( std::isnan( j2v_c.get_value( ) ) );
// set some new values in commands
j1p_c.set_value( 0.11 );
j1v_c.set_value( 0.22 );
j2p_c.set_value( 0.33 );
j2v_c.set_value( 0.44 );
// State values should not be changed
ASSERT_EQ( 3.45, j1p_s.get_value( ) );
ASSERT_EQ( 0.0, j1v_s.get_value( ) );
ASSERT_EQ( 2.78, j2p_s.get_value( ) );
ASSERT_EQ( 0.0, j2v_s.get_value( ) );
ASSERT_EQ( 0.11, j1p_c.get_value( ) );
ASSERT_EQ( 0.22, j1v_c.get_value( ) );
ASSERT_EQ( 0.33, j2p_c.get_value( ) );
ASSERT_EQ( 0.44, j2v_c.get_value( ) );
// write( ) does not change values
rm.write( TIME, PERIOD );
ASSERT_EQ( 3.45, j1p_s.get_value( ) );
ASSERT_EQ( 0.0, j1v_s.get_value( ) );
ASSERT_EQ( 2.78, j2p_s.get_value( ) );
ASSERT_EQ( 0.0, j2v_s.get_value( ) );
ASSERT_EQ( 0.11, j1p_c.get_value( ) );
ASSERT_EQ( 0.22, j1v_c.get_value( ) );
ASSERT_EQ( 0.33, j2p_c.get_value( ) );
ASSERT_EQ( 0.44, j2v_c.get_value( ) );
// read( ) mirrors commands + offset to states
rm.read( TIME, PERIOD );
ASSERT_EQ( 0.11 + offset, j1p_s.get_value( ) );
ASSERT_EQ( 0.22, j1v_s.get_value( ) );
ASSERT_EQ( 0.33 + offset, j2p_s.get_value( ) );
ASSERT_EQ( 0.44, j2v_s.get_value( ) );
ASSERT_EQ( 0.11, j1p_c.get_value( ) );
ASSERT_EQ( 0.22, j1v_c.get_value( ) );
ASSERT_EQ( 0.33, j2p_c.get_value( ) );
ASSERT_EQ( 0.44, j2v_c.get_value( ) );
// set some new values in commands
j1p_c.set_value( 0.55 );
j1v_c.set_value( 0.66 );
j2p_c.set_value( 0.77 );
j2v_c.set_value( 0.88 );
// state values should not be changed
ASSERT_EQ( 0.11 + offset, j1p_s.get_value( ) );
ASSERT_EQ( 0.22, j1v_s.get_value( ) );
ASSERT_EQ( 0.33 + offset, j2p_s.get_value( ) );
ASSERT_EQ( 0.44, j2v_s.get_value( ) );
ASSERT_EQ( 0.55, j1p_c.get_value( ) );
ASSERT_EQ( 0.66, j1v_c.get_value( ) );
ASSERT_EQ( 0.77, j2p_c.get_value( ) );
ASSERT_EQ( 0.88, j2v_c.get_value( ) );
deactivate_components( rm );
status_map = rm.get_components_status( );
EXPECT_EQ(
status_map["GenericSystem2dof"].state.label( ),
hardware_interface::lifecycle_state_names::INACTIVE );
}
TEST_F( TestGenericSystem, generic_system_2dof_functionality )
{
auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_standard_interfaces_ +
ros2_control_test_assets::urdf_tail;
generic_system_functional_test( urdf );
}
TEST_F( TestGenericSystem, generic_system_2dof_other_interfaces )
{
auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_other_interface_ +
ros2_control_test_assets::urdf_tail;
hardware_interface::ResourceManager rm( urdf );
// Activate components to get all interfaces available
activate_components( rm );
// Check interfaces
EXPECT_EQ( 1u, rm.system_components_size( ) );
ASSERT_EQ( 5u, rm.state_interface_keys( ).size( ) );
EXPECT_TRUE( rm.state_interface_exists( "joint1/position" ) );
EXPECT_TRUE( rm.state_interface_exists( "joint1/velocity" ) );
EXPECT_TRUE( rm.state_interface_exists( "joint2/position" ) );
EXPECT_TRUE( rm.state_interface_exists( "joint2/velocity" ) );
EXPECT_TRUE( rm.state_interface_exists( "voltage_output/voltage" ) );
ASSERT_EQ( 5u, rm.command_interface_keys( ).size( ) );
EXPECT_TRUE( rm.command_interface_exists( "joint1/position" ) );
EXPECT_TRUE( rm.command_interface_exists( "joint1/velocity" ) );
EXPECT_TRUE( rm.command_interface_exists( "joint2/position" ) );
EXPECT_TRUE( rm.command_interface_exists( "joint2/velocity" ) );
EXPECT_TRUE( rm.command_interface_exists( "voltage_output/voltage" ) );
// Check initial values
hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface( "joint1/position" );
hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface( "joint1/velocity" );
hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface( "joint2/position" );
hardware_interface::LoanedStateInterface j2v_s = rm.claim_state_interface( "joint2/velocity" );
hardware_interface::LoanedStateInterface vo_s =
rm.claim_state_interface( "voltage_output/voltage" );
hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface( "joint1/position" );
hardware_interface::LoanedCommandInterface j2p_c = rm.claim_command_interface( "joint2/position" );
hardware_interface::LoanedCommandInterface vo_c =
rm.claim_command_interface( "voltage_output/voltage" );
ASSERT_EQ( 1.55, j1p_s.get_value( ) );
ASSERT_EQ( 0.1, j1v_s.get_value( ) );
ASSERT_EQ( 0.65, j2p_s.get_value( ) );
ASSERT_EQ( 0.2, j2v_s.get_value( ) );
ASSERT_EQ( 0.5, vo_s.get_value( ) );
ASSERT_TRUE( std::isnan( j1p_c.get_value( ) ) );
ASSERT_TRUE( std::isnan( j2p_c.get_value( ) ) );
ASSERT_TRUE( std::isnan( vo_c.get_value( ) ) );
// set some new values in commands
j1p_c.set_value( 0.11 );
j2p_c.set_value( 0.33 );
vo_c.set_value( 0.99 );
// State values should not be changed
ASSERT_EQ( 1.55, j1p_s.get_value( ) );
ASSERT_EQ( 0.1, j1v_s.get_value( ) );
ASSERT_EQ( 0.65, j2p_s.get_value( ) );
ASSERT_EQ( 0.2, j2v_s.get_value( ) );
ASSERT_EQ( 0.5, vo_s.get_value( ) );
ASSERT_EQ( 0.11, j1p_c.get_value( ) );
ASSERT_EQ( 0.33, j2p_c.get_value( ) );
ASSERT_EQ( 0.99, vo_c.get_value( ) );
// write( ) does not change values
rm.write( TIME, PERIOD );
ASSERT_EQ( 1.55, j1p_s.get_value( ) );
ASSERT_EQ( 0.1, j1v_s.get_value( ) );
ASSERT_EQ( 0.65, j2p_s.get_value( ) );
ASSERT_EQ( 0.2, j2v_s.get_value( ) );
ASSERT_EQ( 0.5, vo_s.get_value( ) );
ASSERT_EQ( 0.11, j1p_c.get_value( ) );
ASSERT_EQ( 0.33, j2p_c.get_value( ) );
ASSERT_EQ( 0.99, vo_c.get_value( ) );
// read( ) mirrors commands to states
rm.read( TIME, PERIOD );
ASSERT_EQ( 0.11, j1p_s.get_value( ) );
ASSERT_EQ( 0.1, j1v_s.get_value( ) );
ASSERT_EQ( 0.33, j2p_s.get_value( ) );
ASSERT_EQ( 0.99, vo_s.get_value( ) );
ASSERT_EQ( 0.2, j2v_s.get_value( ) );
ASSERT_EQ( 0.11, j1p_c.get_value( ) );
ASSERT_EQ( 0.33, j2p_c.get_value( ) );
ASSERT_EQ( 0.99, vo_c.get_value( ) );
}
TEST_F( TestGenericSystem, generic_system_2dof_sensor )
{
auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_ +
ros2_control_test_assets::urdf_tail;
hardware_interface::ResourceManager rm( urdf );
// Activate components to get all interfaces available
activate_components( rm );
// Check interfaces
EXPECT_EQ( 1u, rm.system_components_size( ) );
ASSERT_EQ( 8u, rm.state_interface_keys( ).size( ) );
EXPECT_TRUE( rm.state_interface_exists( "joint1/position" ) );
EXPECT_TRUE( rm.state_interface_exists( "joint1/velocity" ) );
EXPECT_TRUE( rm.state_interface_exists( "joint2/position" ) );
EXPECT_TRUE( rm.state_interface_exists( "joint2/velocity" ) );
EXPECT_TRUE( rm.state_interface_exists( "tcp_force_sensor/fx" ) );
EXPECT_TRUE( rm.state_interface_exists( "tcp_force_sensor/fy" ) );
EXPECT_TRUE( rm.state_interface_exists( "tcp_force_sensor/tx" ) );
EXPECT_TRUE( rm.state_interface_exists( "tcp_force_sensor/ty" ) );
ASSERT_EQ( 4u, rm.command_interface_keys( ).size( ) );
EXPECT_TRUE( rm.command_interface_exists( "joint1/position" ) );
EXPECT_TRUE( rm.command_interface_exists( "joint1/velocity" ) );
EXPECT_TRUE( rm.command_interface_exists( "joint2/position" ) );
EXPECT_TRUE( rm.command_interface_exists( "joint2/velocity" ) );
EXPECT_FALSE( rm.command_interface_exists( "tcp_force_sensor/fx" ) );
EXPECT_FALSE( rm.command_interface_exists( "tcp_force_sensor/fy" ) );
EXPECT_FALSE( rm.command_interface_exists( "tcp_force_sensor/tx" ) );
EXPECT_FALSE( rm.command_interface_exists( "tcp_force_sensor/ty" ) );
// Check initial values
hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface( "joint1/position" );
hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface( "joint1/velocity" );
hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface( "joint2/position" );
hardware_interface::LoanedStateInterface j2v_s = rm.claim_state_interface( "joint2/velocity" );
hardware_interface::LoanedStateInterface sfx_s = rm.claim_state_interface( "tcp_force_sensor/fx" );
hardware_interface::LoanedStateInterface sfy_s = rm.claim_state_interface( "tcp_force_sensor/fy" );
hardware_interface::LoanedStateInterface stx_s = rm.claim_state_interface( "tcp_force_sensor/tx" );
hardware_interface::LoanedStateInterface sty_s = rm.claim_state_interface( "tcp_force_sensor/ty" );
hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface( "joint1/position" );
hardware_interface::LoanedCommandInterface j2p_c = rm.claim_command_interface( "joint2/position" );
EXPECT_ANY_THROW( rm.claim_command_interface( "tcp_force_sensor/fx" ) );
EXPECT_ANY_THROW( rm.claim_command_interface( "tcp_force_sensor/fy" ) );
EXPECT_ANY_THROW( rm.claim_command_interface( "tcp_force_sensor/tx" ) );
EXPECT_ANY_THROW( rm.claim_command_interface( "tcp_force_sensor/ty" ) );
ASSERT_EQ( 0.0, j1p_s.get_value( ) );
ASSERT_EQ( 0.0, j1v_s.get_value( ) );
ASSERT_EQ( 0.0, j2p_s.get_value( ) );
ASSERT_EQ( 0.0, j2v_s.get_value( ) );
EXPECT_TRUE( std::isnan( sfx_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( sfy_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( stx_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( sty_s.get_value( ) ) );
ASSERT_TRUE( std::isnan( j1p_c.get_value( ) ) );
ASSERT_TRUE( std::isnan( j2p_c.get_value( ) ) );
// set some new values in commands
j1p_c.set_value( 0.11 );
j2p_c.set_value( 0.33 );
// State values should not be changed
ASSERT_EQ( 0.0, j1p_s.get_value( ) );
ASSERT_EQ( 0.0, j1v_s.get_value( ) );
ASSERT_EQ( 0.0, j2p_s.get_value( ) );
ASSERT_EQ( 0.0, j2v_s.get_value( ) );
EXPECT_TRUE( std::isnan( sfx_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( sfy_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( stx_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( sty_s.get_value( ) ) );
ASSERT_EQ( 0.11, j1p_c.get_value( ) );
ASSERT_EQ( 0.33, j2p_c.get_value( ) );
// write( ) does not change values
rm.write( TIME, PERIOD );
ASSERT_EQ( 0.0, j1p_s.get_value( ) );
ASSERT_EQ( 0.0, j1v_s.get_value( ) );
ASSERT_EQ( 0.0, j2p_s.get_value( ) );
ASSERT_EQ( 0.0, j2v_s.get_value( ) );
EXPECT_TRUE( std::isnan( sfx_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( sfy_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( stx_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( sty_s.get_value( ) ) );
ASSERT_EQ( 0.11, j1p_c.get_value( ) );
ASSERT_EQ( 0.33, j2p_c.get_value( ) );
// read( ) mirrors commands to states
rm.read( TIME, PERIOD );
ASSERT_EQ( 0.11, j1p_s.get_value( ) );
ASSERT_EQ( 0.0, j1v_s.get_value( ) );
ASSERT_EQ( 0.33, j2p_s.get_value( ) );
EXPECT_TRUE( std::isnan( sfx_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( sfy_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( stx_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( sty_s.get_value( ) ) );
ASSERT_EQ( 0.0, j2v_s.get_value( ) );
ASSERT_EQ( 0.11, j1p_c.get_value( ) );
ASSERT_EQ( 0.33, j2p_c.get_value( ) );
}
void test_generic_system_with_fake_sensor_commands( std::string urdf )
{
hardware_interface::ResourceManager rm( urdf );
// Activate components to get all interfaces available
activate_components( rm );
// Check interfaces
EXPECT_EQ( 1u, rm.system_components_size( ) );
ASSERT_EQ( 8u, rm.state_interface_keys( ).size( ) );
EXPECT_TRUE( rm.state_interface_exists( "joint1/position" ) );
EXPECT_TRUE( rm.state_interface_exists( "joint1/velocity" ) );
EXPECT_TRUE( rm.state_interface_exists( "joint2/position" ) );
EXPECT_TRUE( rm.state_interface_exists( "joint2/velocity" ) );
EXPECT_TRUE( rm.state_interface_exists( "tcp_force_sensor/fx" ) );
EXPECT_TRUE( rm.state_interface_exists( "tcp_force_sensor/fy" ) );
EXPECT_TRUE( rm.state_interface_exists( "tcp_force_sensor/tx" ) );
EXPECT_TRUE( rm.state_interface_exists( "tcp_force_sensor/ty" ) );
ASSERT_EQ( 8u, rm.command_interface_keys( ).size( ) );
EXPECT_TRUE( rm.command_interface_exists( "joint1/position" ) );
EXPECT_TRUE( rm.command_interface_exists( "joint1/velocity" ) );
EXPECT_TRUE( rm.command_interface_exists( "joint2/position" ) );
EXPECT_TRUE( rm.command_interface_exists( "joint2/velocity" ) );
EXPECT_TRUE( rm.command_interface_exists( "tcp_force_sensor/fx" ) );
EXPECT_TRUE( rm.command_interface_exists( "tcp_force_sensor/fy" ) );
EXPECT_TRUE( rm.command_interface_exists( "tcp_force_sensor/tx" ) );
EXPECT_TRUE( rm.command_interface_exists( "tcp_force_sensor/ty" ) );
// Check initial values
hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface( "joint1/position" );
hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface( "joint1/velocity" );
hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface( "joint2/position" );
hardware_interface::LoanedStateInterface j2v_s = rm.claim_state_interface( "joint2/velocity" );
hardware_interface::LoanedStateInterface sfx_s = rm.claim_state_interface( "tcp_force_sensor/fx" );
hardware_interface::LoanedStateInterface sfy_s = rm.claim_state_interface( "tcp_force_sensor/fy" );
hardware_interface::LoanedStateInterface stx_s = rm.claim_state_interface( "tcp_force_sensor/tx" );
hardware_interface::LoanedStateInterface sty_s = rm.claim_state_interface( "tcp_force_sensor/ty" );
hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface( "joint1/position" );
hardware_interface::LoanedCommandInterface j2p_c = rm.claim_command_interface( "joint2/position" );
hardware_interface::LoanedCommandInterface sfx_c =
rm.claim_command_interface( "tcp_force_sensor/fx" );
hardware_interface::LoanedCommandInterface sfy_c =
rm.claim_command_interface( "tcp_force_sensor/fy" );
hardware_interface::LoanedCommandInterface stx_c =
rm.claim_command_interface( "tcp_force_sensor/tx" );
hardware_interface::LoanedCommandInterface sty_c =
rm.claim_command_interface( "tcp_force_sensor/ty" );
ASSERT_EQ( 0.0, j1p_s.get_value( ) );
ASSERT_EQ( 0.0, j1v_s.get_value( ) );
ASSERT_EQ( 0.0, j2p_s.get_value( ) );
ASSERT_EQ( 0.0, j2v_s.get_value( ) );
EXPECT_TRUE( std::isnan( sfx_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( sfy_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( stx_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( sty_s.get_value( ) ) );
ASSERT_TRUE( std::isnan( j1p_c.get_value( ) ) );
ASSERT_TRUE( std::isnan( j2p_c.get_value( ) ) );
EXPECT_TRUE( std::isnan( sfx_c.get_value( ) ) );
EXPECT_TRUE( std::isnan( sfy_c.get_value( ) ) );
EXPECT_TRUE( std::isnan( stx_c.get_value( ) ) );
EXPECT_TRUE( std::isnan( sty_c.get_value( ) ) );
// set some new values in commands
j1p_c.set_value( 0.11 );
j2p_c.set_value( 0.33 );
sfx_c.set_value( 1.11 );
sfy_c.set_value( 2.22 );
stx_c.set_value( 3.33 );
sty_c.set_value( 4.44 );
// State values should not be changed
ASSERT_EQ( 0.0, j1p_s.get_value( ) );
ASSERT_EQ( 0.0, j1v_s.get_value( ) );
ASSERT_EQ( 0.0, j2p_s.get_value( ) );
ASSERT_EQ( 0.0, j2v_s.get_value( ) );
EXPECT_TRUE( std::isnan( sfx_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( sfy_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( stx_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( sty_s.get_value( ) ) );
ASSERT_EQ( 0.11, j1p_c.get_value( ) );
ASSERT_EQ( 0.33, j2p_c.get_value( ) );
ASSERT_EQ( 1.11, sfx_c.get_value( ) );
ASSERT_EQ( 2.22, sfy_c.get_value( ) );
ASSERT_EQ( 3.33, stx_c.get_value( ) );
ASSERT_EQ( 4.44, sty_c.get_value( ) );
// write( ) does not change values
rm.write( TIME, PERIOD );
ASSERT_EQ( 0.0, j1p_s.get_value( ) );
ASSERT_EQ( 0.0, j1v_s.get_value( ) );
ASSERT_EQ( 0.0, j2p_s.get_value( ) );
ASSERT_EQ( 0.0, j2v_s.get_value( ) );
EXPECT_TRUE( std::isnan( sfx_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( sfy_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( stx_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( sty_s.get_value( ) ) );
ASSERT_EQ( 0.11, j1p_c.get_value( ) );
ASSERT_EQ( 0.33, j2p_c.get_value( ) );
ASSERT_EQ( 1.11, sfx_c.get_value( ) );
ASSERT_EQ( 2.22, sfy_c.get_value( ) );
ASSERT_EQ( 3.33, stx_c.get_value( ) );
ASSERT_EQ( 4.44, sty_c.get_value( ) );
// read( ) mirrors commands to states
rm.read( TIME, PERIOD );
ASSERT_EQ( 0.11, j1p_s.get_value( ) );
ASSERT_EQ( 0.0, j1v_s.get_value( ) );
ASSERT_EQ( 0.33, j2p_s.get_value( ) );
ASSERT_EQ( 0.0, j2v_s.get_value( ) );
ASSERT_EQ( 1.11, sfx_s.get_value( ) );
ASSERT_EQ( 2.22, sfy_s.get_value( ) );
ASSERT_EQ( 3.33, stx_s.get_value( ) );
ASSERT_EQ( 4.44, sty_s.get_value( ) );
ASSERT_EQ( 0.11, j1p_c.get_value( ) );
ASSERT_EQ( 0.33, j2p_c.get_value( ) );
ASSERT_EQ( 1.11, sfx_c.get_value( ) );
ASSERT_EQ( 2.22, sfy_c.get_value( ) );
ASSERT_EQ( 3.33, stx_c.get_value( ) );
ASSERT_EQ( 4.44, sty_c.get_value( ) );
}
TEST_F( TestGenericSystem, generic_system_2dof_sensor_fake_command )
{
auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_sensor_fake_command_ +
ros2_control_test_assets::urdf_tail;
test_generic_system_with_fake_sensor_commands( urdf );
}
TEST_F( TestGenericSystem, generic_system_2dof_sensor_fake_command_True )
{
auto urdf = ros2_control_test_assets::urdf_head +
hardware_system_2dof_with_sensor_fake_command_True_ +
ros2_control_test_assets::urdf_tail;
test_generic_system_with_fake_sensor_commands( urdf );
}
void test_generic_system_with_mimic_joint( std::string urdf )
{
hardware_interface::ResourceManager rm( urdf );
// Activate components to get all interfaces available
activate_components( rm );
// Check interfaces
EXPECT_EQ( 1u, rm.system_components_size( ) );
ASSERT_EQ( 4u, rm.state_interface_keys( ).size( ) );
EXPECT_TRUE( rm.state_interface_exists( "joint1/position" ) );
EXPECT_TRUE( rm.state_interface_exists( "joint1/velocity" ) );
EXPECT_TRUE( rm.state_interface_exists( "joint2/position" ) );
EXPECT_TRUE( rm.state_interface_exists( "joint2/velocity" ) );
ASSERT_EQ( 4u, rm.command_interface_keys( ).size( ) );
EXPECT_TRUE( rm.command_interface_exists( "joint1/position" ) );
EXPECT_TRUE( rm.command_interface_exists( "joint1/velocity" ) );
EXPECT_TRUE( rm.command_interface_exists( "joint2/position" ) );
EXPECT_TRUE( rm.command_interface_exists( "joint2/velocity" ) );
// Check initial values
hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface( "joint1/position" );
hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface( "joint1/velocity" );
hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface( "joint2/position" );
hardware_interface::LoanedStateInterface j2v_s = rm.claim_state_interface( "joint2/velocity" );
hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface( "joint1/position" );
hardware_interface::LoanedCommandInterface j1v_c = rm.claim_command_interface( "joint1/velocity" );
ASSERT_EQ( 1.57, j1p_s.get_value( ) );
ASSERT_EQ( 0.0, j1v_s.get_value( ) );
ASSERT_EQ( 0.0, j2p_s.get_value( ) );
ASSERT_EQ( 0.0, j2v_s.get_value( ) );
ASSERT_TRUE( std::isnan( j1p_c.get_value( ) ) );
ASSERT_TRUE( std::isnan( j1v_c.get_value( ) ) );
// set some new values in commands
j1p_c.set_value( 0.11 );
j1v_c.set_value( 0.05 );
// State values should not be changed
ASSERT_EQ( 1.57, j1p_s.get_value( ) );
ASSERT_EQ( 0.0, j1v_s.get_value( ) );
ASSERT_EQ( 0.0, j2p_s.get_value( ) );
ASSERT_EQ( 0.0, j2v_s.get_value( ) );
ASSERT_EQ( 0.11, j1p_c.get_value( ) );
ASSERT_EQ( 0.05, j1v_c.get_value( ) );
// write( ) does not change values
rm.write( TIME, PERIOD );
ASSERT_EQ( 1.57, j1p_s.get_value( ) );
ASSERT_EQ( 0.0, j1v_s.get_value( ) );
ASSERT_EQ( 0.0, j2p_s.get_value( ) );
ASSERT_EQ( 0.0, j2v_s.get_value( ) );
ASSERT_EQ( 0.11, j1p_c.get_value( ) );
ASSERT_EQ( 0.05, j1v_c.get_value( ) );
// read( ) mirrors commands to states
rm.read( TIME, PERIOD );
ASSERT_EQ( 0.11, j1p_s.get_value( ) );
ASSERT_EQ( 0.05, j1v_s.get_value( ) );
ASSERT_EQ( -0.22, j2p_s.get_value( ) );
ASSERT_EQ( -0.1, j2v_s.get_value( ) );
ASSERT_EQ( 0.11, j1p_c.get_value( ) );
ASSERT_EQ( 0.05, j1v_c.get_value( ) );
}
TEST_F( TestGenericSystem, hardware_system_2dof_with_mimic_joint )
{
auto urdf = ros2_control_test_assets::urdf_head + hardware_system_2dof_with_mimic_joint_ +
ros2_control_test_assets::urdf_tail;
test_generic_system_with_mimic_joint( urdf );
}
TEST_F( TestGenericSystem, generic_system_2dof_functionality_with_offset )
{
auto urdf = ros2_control_test_assets::urdf_head +
hardware_system_2dof_standard_interfaces_with_offset_ +
ros2_control_test_assets::urdf_tail;
generic_system_functional_test( urdf, -3 );
}
TEST_F( TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_interface_missing )
{
auto urdf = ros2_control_test_assets::urdf_head +
hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_missing_ +
ros2_control_test_assets::urdf_tail;
// custom interface is missing so offset will not be applied
generic_system_functional_test( urdf, 0.0 );
}
TEST_F( TestGenericSystem, generic_system_2dof_functionality_with_offset_custom_interface )
{
auto urdf = ros2_control_test_assets::urdf_head +
hardware_system_2dof_standard_interfaces_with_custom_interface_for_offset_ +
ros2_control_test_assets::urdf_tail;
const double offset = -3;
hardware_interface::ResourceManager rm( urdf );
// check is hardware is configured
auto status_map = rm.get_components_status( );
EXPECT_EQ(
status_map["GenericSystem2dof"].state.label( ),
hardware_interface::lifecycle_state_names::UNCONFIGURED );
configure_components( rm );
status_map = rm.get_components_status( );
EXPECT_EQ(
status_map["GenericSystem2dof"].state.label( ),
hardware_interface::lifecycle_state_names::INACTIVE );
activate_components( rm );
status_map = rm.get_components_status( );
EXPECT_EQ(
status_map["GenericSystem2dof"].state.label( ),
hardware_interface::lifecycle_state_names::ACTIVE );
// Check initial values
hardware_interface::LoanedStateInterface j1p_s = rm.claim_state_interface( "joint1/position" );
hardware_interface::LoanedStateInterface j1v_s = rm.claim_state_interface( "joint1/velocity" );
hardware_interface::LoanedStateInterface j2p_s = rm.claim_state_interface( "joint2/position" );
hardware_interface::LoanedStateInterface j2v_s = rm.claim_state_interface( "joint2/velocity" );
hardware_interface::LoanedCommandInterface j1p_c = rm.claim_command_interface( "joint1/position" );
hardware_interface::LoanedCommandInterface j1v_c = rm.claim_command_interface( "joint1/velocity" );
hardware_interface::LoanedCommandInterface j2p_c = rm.claim_command_interface( "joint2/position" );
hardware_interface::LoanedCommandInterface j2v_c = rm.claim_command_interface( "joint2/velocity" );
// set default value of custom state interfaces to anything first
hardware_interface::LoanedStateInterface c_j1p_s =
rm.claim_state_interface( "joint1/actual_position" );
hardware_interface::LoanedStateInterface c_j2p_s =
rm.claim_state_interface( "joint2/actual_position" );
// State interfaces without initial value are set to 0
ASSERT_EQ( 3.45, j1p_s.get_value( ) );
ASSERT_EQ( 0.0, j1v_s.get_value( ) );
ASSERT_EQ( 2.78, j2p_s.get_value( ) );
ASSERT_EQ( 0.0, j2v_s.get_value( ) );
ASSERT_TRUE( std::isnan( j1p_c.get_value( ) ) );
ASSERT_TRUE( std::isnan( j1v_c.get_value( ) ) );
ASSERT_TRUE( std::isnan( j2p_c.get_value( ) ) );
ASSERT_TRUE( std::isnan( j2v_c.get_value( ) ) );
// set some new values in commands
j1p_c.set_value( 0.11 );
j1v_c.set_value( 0.22 );
j2p_c.set_value( 0.33 );
j2v_c.set_value( 0.44 );
// State values should not be changed
ASSERT_EQ( 3.45, j1p_s.get_value( ) );
ASSERT_EQ( 0.0, j1v_s.get_value( ) );
ASSERT_EQ( 2.78, j2p_s.get_value( ) );
ASSERT_EQ( 0.0, j2v_s.get_value( ) );
ASSERT_EQ( 0.11, j1p_c.get_value( ) );
ASSERT_EQ( 0.22, j1v_c.get_value( ) );
ASSERT_EQ( 0.33, j2p_c.get_value( ) );
ASSERT_EQ( 0.44, j2v_c.get_value( ) );
// write( ) does not change values
rm.write( TIME, PERIOD );
ASSERT_EQ( 3.45, j1p_s.get_value( ) );
ASSERT_EQ( 0.0, j1v_s.get_value( ) );
ASSERT_EQ( 2.78, j2p_s.get_value( ) );
ASSERT_EQ( 0.0, j2v_s.get_value( ) );
ASSERT_EQ( 0.11, j1p_c.get_value( ) );
ASSERT_EQ( 0.22, j1v_c.get_value( ) );
ASSERT_EQ( 0.33, j2p_c.get_value( ) );
ASSERT_EQ( 0.44, j2v_c.get_value( ) );
// read( ) mirrors commands + offset to states
rm.read( TIME, PERIOD );
ASSERT_EQ( 0.11, j1p_s.get_value( ) );
ASSERT_EQ( 0.11 + offset, c_j1p_s.get_value( ) );
ASSERT_EQ( 0.22, j1v_s.get_value( ) );
ASSERT_EQ( 0.33, j2p_s.get_value( ) );
ASSERT_EQ( 0.33 + offset, c_j2p_s.get_value( ) );
ASSERT_EQ( 0.44, j2v_s.get_value( ) );
ASSERT_EQ( 0.11, j1p_c.get_value( ) );
ASSERT_EQ( 0.22, j1v_c.get_value( ) );
ASSERT_EQ( 0.33, j2p_c.get_value( ) );
ASSERT_EQ( 0.44, j2v_c.get_value( ) );
// set some new values in commands
j1p_c.set_value( 0.55 );
j1v_c.set_value( 0.66 );
j2p_c.set_value( 0.77 );
j2v_c.set_value( 0.88 );
// state values should not be changed
ASSERT_EQ( 0.11, j1p_s.get_value( ) );
ASSERT_EQ( 0.11 + offset, c_j1p_s.get_value( ) );
ASSERT_EQ( 0.22, j1v_s.get_value( ) );
ASSERT_EQ( 0.33, j2p_s.get_value( ) );
ASSERT_EQ( 0.33 + offset, c_j2p_s.get_value( ) );
ASSERT_EQ( 0.44, j2v_s.get_value( ) );
ASSERT_EQ( 0.55, j1p_c.get_value( ) );
ASSERT_EQ( 0.66, j1v_c.get_value( ) );
ASSERT_EQ( 0.77, j2p_c.get_value( ) );
ASSERT_EQ( 0.88, j2v_c.get_value( ) );
deactivate_components( rm );
status_map = rm.get_components_status( );
EXPECT_EQ(
status_map["GenericSystem2dof"].state.label( ),
hardware_interface::lifecycle_state_names::INACTIVE );
}
TEST_F( TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_ )
{
auto urdf = ros2_control_test_assets::urdf_head +
valid_urdf_ros2_control_system_robot_with_gpio_ + ros2_control_test_assets::urdf_tail;
hardware_interface::ResourceManager rm( urdf );
// check is hardware is started
auto status_map = rm.get_components_status( );
EXPECT_EQ(
status_map["GenericSystem2dof"].state.label( ),
hardware_interface::lifecycle_state_names::UNCONFIGURED );
configure_components( rm );
status_map = rm.get_components_status( );
EXPECT_EQ(
status_map["GenericSystem2dof"].state.label( ),
hardware_interface::lifecycle_state_names::INACTIVE );
activate_components( rm );
status_map = rm.get_components_status( );
EXPECT_EQ(
status_map["GenericSystem2dof"].state.label( ),
hardware_interface::lifecycle_state_names::ACTIVE );
ASSERT_EQ( 8u, rm.state_interface_keys( ).size( ) );
ASSERT_EQ( 6u, rm.command_interface_keys( ).size( ) );
EXPECT_TRUE( rm.state_interface_exists( "joint1/position" ) );
EXPECT_TRUE( rm.state_interface_exists( "joint1/velocity" ) );
EXPECT_TRUE( rm.state_interface_exists( "joint2/position" ) );
EXPECT_TRUE( rm.state_interface_exists( "joint2/velocity" ) );
EXPECT_TRUE( rm.state_interface_exists( "flange_analog_IOs/analog_output1" ) );
EXPECT_TRUE( rm.state_interface_exists( "flange_analog_IOs/analog_input1" ) );
EXPECT_TRUE( rm.state_interface_exists( "flange_analog_IOs/analog_input2" ) );
EXPECT_TRUE( rm.state_interface_exists( "flange_vacuum/vacuum" ) );
EXPECT_TRUE( rm.command_interface_exists( "joint1/position" ) );
EXPECT_TRUE( rm.command_interface_exists( "joint1/velocity" ) );
EXPECT_TRUE( rm.command_interface_exists( "joint2/position" ) );
EXPECT_TRUE( rm.command_interface_exists( "joint2/velocity" ) );
EXPECT_TRUE( rm.command_interface_exists( "flange_analog_IOs/analog_output1" ) );
EXPECT_TRUE( rm.command_interface_exists( "flange_vacuum/vacuum" ) );
// Check initial values
hardware_interface::LoanedStateInterface gpio1_a_o1_s =
rm.claim_state_interface( "flange_analog_IOs/analog_output1" );
hardware_interface::LoanedStateInterface gpio1_a_i1_s =
rm.claim_state_interface( "flange_analog_IOs/analog_input1" );
hardware_interface::LoanedStateInterface gpio1_a_o2_s =
rm.claim_state_interface( "flange_analog_IOs/analog_input2" );
hardware_interface::LoanedStateInterface gpio2_vac_s =
rm.claim_state_interface( "flange_vacuum/vacuum" );
hardware_interface::LoanedCommandInterface gpio1_a_o1_c =
rm.claim_command_interface( "flange_analog_IOs/analog_output1" );
hardware_interface::LoanedCommandInterface gpio2_vac_c =
rm.claim_command_interface( "flange_vacuum/vacuum" );
// State interfaces without initial value are set to 0
ASSERT_TRUE( std::isnan( gpio1_a_o1_s.get_value( ) ) );
ASSERT_TRUE( std::isnan( gpio2_vac_s.get_value( ) ) );
ASSERT_TRUE( std::isnan( gpio1_a_o1_c.get_value( ) ) );
ASSERT_TRUE( std::isnan( gpio2_vac_c.get_value( ) ) );
// set some new values in commands
gpio1_a_o1_c.set_value( 0.111 );
gpio2_vac_c.set_value( 0.222 );
// State values should not be changed
ASSERT_TRUE( std::isnan( gpio1_a_o1_s.get_value( ) ) );
ASSERT_TRUE( std::isnan( gpio2_vac_s.get_value( ) ) );
ASSERT_EQ( 0.111, gpio1_a_o1_c.get_value( ) );
ASSERT_EQ( 0.222, gpio2_vac_c.get_value( ) );
// write( ) does not change values
rm.write( TIME, PERIOD );
ASSERT_TRUE( std::isnan( gpio1_a_o1_s.get_value( ) ) );
ASSERT_TRUE( std::isnan( gpio2_vac_s.get_value( ) ) );
ASSERT_EQ( 0.111, gpio1_a_o1_c.get_value( ) );
ASSERT_EQ( 0.222, gpio2_vac_c.get_value( ) );
// read( ) mirrors commands + offset to states
rm.read( TIME, PERIOD );
ASSERT_EQ( 0.111, gpio1_a_o1_s.get_value( ) );
ASSERT_EQ( 0.222, gpio2_vac_s.get_value( ) );
ASSERT_EQ( 0.111, gpio1_a_o1_c.get_value( ) );
ASSERT_EQ( 0.222, gpio2_vac_c.get_value( ) );
// set some new values in commands
gpio1_a_o1_c.set_value( 0.333 );
gpio2_vac_c.set_value( 0.444 );
// state values should not be changed
ASSERT_EQ( 0.111, gpio1_a_o1_s.get_value( ) );
ASSERT_EQ( 0.222, gpio2_vac_s.get_value( ) );
ASSERT_EQ( 0.333, gpio1_a_o1_c.get_value( ) );
ASSERT_EQ( 0.444, gpio2_vac_c.get_value( ) );
// check other functionalities are working well
generic_system_functional_test( urdf );
}
TEST_F( TestGenericSystem, valid_urdf_ros2_control_system_robot_with_gpio_fake_command_ )
{
auto urdf = ros2_control_test_assets::urdf_head +
valid_urdf_ros2_control_system_robot_with_gpio_fake_command_ +
ros2_control_test_assets::urdf_tail;
hardware_interface::ResourceManager rm( urdf );
// check is hardware is started
auto status_map = rm.get_components_status( );
EXPECT_EQ(
status_map["GenericSystem2dof"].state.label( ),
hardware_interface::lifecycle_state_names::UNCONFIGURED );
configure_components( rm );
status_map = rm.get_components_status( );
EXPECT_EQ(
status_map["GenericSystem2dof"].state.label( ),
hardware_interface::lifecycle_state_names::INACTIVE );
activate_components( rm );
status_map = rm.get_components_status( );
EXPECT_EQ(
status_map["GenericSystem2dof"].state.label( ),
hardware_interface::lifecycle_state_names::ACTIVE );
// Check interfaces
EXPECT_EQ( 1u, rm.system_components_size( ) );
ASSERT_EQ( 8u, rm.state_interface_keys( ).size( ) );
EXPECT_TRUE( rm.state_interface_exists( "joint1/position" ) );
EXPECT_TRUE( rm.state_interface_exists( "joint1/velocity" ) );
EXPECT_TRUE( rm.state_interface_exists( "joint2/position" ) );
EXPECT_TRUE( rm.state_interface_exists( "joint2/velocity" ) );
EXPECT_TRUE( rm.state_interface_exists( "flange_analog_IOs/analog_output1" ) );
EXPECT_TRUE( rm.state_interface_exists( "flange_analog_IOs/analog_input1" ) );
EXPECT_TRUE( rm.state_interface_exists( "flange_analog_IOs/analog_input2" ) );
EXPECT_TRUE( rm.state_interface_exists( "flange_vacuum/vacuum" ) );
ASSERT_EQ( 8u, rm.command_interface_keys( ).size( ) );
EXPECT_TRUE( rm.command_interface_exists( "joint1/position" ) );
EXPECT_TRUE( rm.command_interface_exists( "joint1/velocity" ) );
EXPECT_TRUE( rm.command_interface_exists( "joint2/position" ) );
EXPECT_TRUE( rm.command_interface_exists( "joint2/velocity" ) );
EXPECT_TRUE( rm.command_interface_exists( "flange_analog_IOs/analog_output1" ) );
EXPECT_TRUE( rm.command_interface_exists( "flange_analog_IOs/analog_input1" ) );
EXPECT_TRUE( rm.command_interface_exists( "flange_analog_IOs/analog_input2" ) );
EXPECT_TRUE( rm.command_interface_exists( "flange_vacuum/vacuum" ) );
// Check initial values
hardware_interface::LoanedStateInterface gpio1_a_o1_s =
rm.claim_state_interface( "flange_analog_IOs/analog_output1" );
hardware_interface::LoanedStateInterface gpio1_a_i1_s =
rm.claim_state_interface( "flange_analog_IOs/analog_input1" );
hardware_interface::LoanedStateInterface gpio1_a_o2_s =
rm.claim_state_interface( "flange_analog_IOs/analog_input2" );
hardware_interface::LoanedStateInterface gpio2_vac_s =
rm.claim_state_interface( "flange_vacuum/vacuum" );
hardware_interface::LoanedCommandInterface gpio1_a_o1_c =
rm.claim_command_interface( "flange_analog_IOs/analog_output1" );
hardware_interface::LoanedCommandInterface gpio1_a_i1_c =
rm.claim_command_interface( "flange_analog_IOs/analog_input1" );
hardware_interface::LoanedCommandInterface gpio1_a_i2_c =
rm.claim_command_interface( "flange_analog_IOs/analog_input2" );
hardware_interface::LoanedCommandInterface gpio2_vac_c =
rm.claim_command_interface( "flange_vacuum/vacuum" );
EXPECT_TRUE( std::isnan( gpio1_a_o1_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( gpio1_a_i1_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( gpio1_a_o2_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( gpio2_vac_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( gpio1_a_o1_c.get_value( ) ) );
EXPECT_TRUE( std::isnan( gpio1_a_i1_c.get_value( ) ) );
EXPECT_TRUE( std::isnan( gpio1_a_i2_c.get_value( ) ) );
EXPECT_TRUE( std::isnan( gpio2_vac_c.get_value( ) ) );
// set some new values in commands
gpio1_a_o1_c.set_value( 0.11 );
gpio1_a_i1_c.set_value( 0.33 );
gpio1_a_i2_c.set_value( 1.11 );
gpio2_vac_c.set_value( 2.22 );
// State values should not be changed
EXPECT_TRUE( std::isnan( gpio1_a_o1_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( gpio1_a_i1_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( gpio1_a_o2_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( gpio2_vac_s.get_value( ) ) );
ASSERT_EQ( 0.11, gpio1_a_o1_c.get_value( ) );
ASSERT_EQ( 0.33, gpio1_a_i1_c.get_value( ) );
ASSERT_EQ( 1.11, gpio1_a_i2_c.get_value( ) );
ASSERT_EQ( 2.22, gpio2_vac_c.get_value( ) );
// write( ) does not change values
rm.write( TIME, PERIOD );
EXPECT_TRUE( std::isnan( gpio1_a_o1_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( gpio1_a_i1_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( gpio1_a_o2_s.get_value( ) ) );
EXPECT_TRUE( std::isnan( gpio2_vac_s.get_value( ) ) );
ASSERT_EQ( 0.11, gpio1_a_o1_c.get_value( ) );
ASSERT_EQ( 0.33, gpio1_a_i1_c.get_value( ) );
ASSERT_EQ( 1.11, gpio1_a_i2_c.get_value( ) );
ASSERT_EQ( 2.22, gpio2_vac_c.get_value( ) );
// read( ) mirrors commands to states
rm.read( TIME, PERIOD );
ASSERT_EQ( 0.11, gpio1_a_o1_s.get_value( ) );
ASSERT_EQ( 0.33, gpio1_a_i1_s.get_value( ) );
ASSERT_EQ( 1.11, gpio1_a_o2_s.get_value( ) );
ASSERT_EQ( 2.22, gpio2_vac_s.get_value( ) );
ASSERT_EQ( 0.11, gpio1_a_o1_c.get_value( ) );
ASSERT_EQ( 0.33, gpio1_a_i1_c.get_value( ) );
ASSERT_EQ( 1.11, gpio1_a_i2_c.get_value( ) );
ASSERT_EQ( 2.22, gpio2_vac_c.get_value( ) );
}
// Copyright 2020 ros2_control development team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gmock/gmock.h>
#include <array>
#include <limits>
#include <memory>
#include <string>
#include <unordered_map>
#include <vector>
#include "hardware_interface/actuator.hpp"
#include "hardware_interface/actuator_interface.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/sensor.hpp"
#include "hardware_interface/sensor_interface.hpp"
#include "hardware_interface/system.hpp"
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
// Values to send over command interface to trigger error in write and read methods
namespace
{
41 const auto TIME = rclcpp::Time( 0 );
const auto PERIOD = rclcpp::Duration::from_seconds( 0.01 );
constexpr unsigned int TRIGGER_READ_WRITE_ERROR_CALLS = 10000;
} // namespace
using namespace ::testing; // NOLINT
namespace test_components
{
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
class DummyActuator : public hardware_interface::ActuatorInterface
{
CallbackReturn on_init( const hardware_interface::HardwareInfo & /*info*/ ) override
{
// We hardcode the info
return CallbackReturn::SUCCESS;
}
CallbackReturn on_configure( const rclcpp_lifecycle::State & /*previous_state*/ ) override
{
position_state_ = 0.0;
velocity_state_ = 0.0;
if ( recoverable_error_happened_ )
{
velocity_command_ = 0.0;
}
read_calls_ = 0;
write_calls_ = 0;
return CallbackReturn::SUCCESS;
}
std::vector<hardware_interface::StateInterface> export_state_interfaces( ) override
{
// We can read a position and a velocity
std::vector<hardware_interface::StateInterface> state_interfaces;
state_interfaces.emplace_back( hardware_interface::StateInterface(
"joint1", hardware_interface::HW_IF_POSITION, &position_state_ ) );
state_interfaces.emplace_back( hardware_interface::StateInterface(
"joint1", hardware_interface::HW_IF_VELOCITY, &velocity_state_ ) );
return state_interfaces;
}
std::vector<hardware_interface::CommandInterface> export_command_interfaces( ) override
{
// We can command in velocity
std::vector<hardware_interface::CommandInterface> command_interfaces;
command_interfaces.emplace_back( hardware_interface::CommandInterface(
"joint1", hardware_interface::HW_IF_VELOCITY, &velocity_command_ ) );
return command_interfaces;
}
std::string get_name( ) const override { return "DummyActuator"; }
hardware_interface::return_type read(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) override
{
++read_calls_;
if ( read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS )
{
return hardware_interface::return_type::ERROR;
}
// no-op, state is getting propagated within write.
return hardware_interface::return_type::OK;
}
hardware_interface::return_type write(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) override
{
++write_calls_;
if ( write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS )
{
return hardware_interface::return_type::ERROR;
}
position_state_ += velocity_command_;
velocity_state_ = velocity_command_;
return hardware_interface::return_type::OK;
}
CallbackReturn on_shutdown( const rclcpp_lifecycle::State & /*previous_state*/ ) override
{
velocity_state_ = 0;
return CallbackReturn::SUCCESS;
}
CallbackReturn on_error( const rclcpp_lifecycle::State & /*previous_state*/ ) override
{
if ( !recoverable_error_happened_ )
{
recoverable_error_happened_ = true;
return CallbackReturn::SUCCESS;
}
else
{
return CallbackReturn::ERROR;
}
return CallbackReturn::FAILURE;
}
private:
double position_state_ = std::numeric_limits<double>::quiet_NaN( );
double velocity_state_ = std::numeric_limits<double>::quiet_NaN( );
double velocity_command_ = 0.0;
// Helper variables to initiate error on read
unsigned int read_calls_ = 0;
unsigned int write_calls_ = 0;
bool recoverable_error_happened_ = false;
};
class DummySensor : public hardware_interface::SensorInterface
{
CallbackReturn on_init( const hardware_interface::HardwareInfo & /*info*/ ) override
{
// We hardcode the info
return CallbackReturn::SUCCESS;
}
CallbackReturn on_configure( const rclcpp_lifecycle::State & /*previous_state*/ ) override
{
voltage_level_ = 0.0;
read_calls_ = 0;
return CallbackReturn::SUCCESS;
}
std::vector<hardware_interface::StateInterface> export_state_interfaces( ) override
{
// We can read some voltage level
std::vector<hardware_interface::StateInterface> state_interfaces;
state_interfaces.emplace_back(
hardware_interface::StateInterface( "joint1", "voltage", &voltage_level_ ) );
return state_interfaces;
}
std::string get_name( ) const override { return "DummySensor"; }
hardware_interface::return_type read(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) override
{
++read_calls_;
if ( read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS )
{
return hardware_interface::return_type::ERROR;
}
// no-op, static value
voltage_level_ = voltage_level_hw_value_;
return hardware_interface::return_type::OK;
}
CallbackReturn on_error( const rclcpp_lifecycle::State & /*previous_state*/ ) override
{
if ( !recoverable_error_happened_ )
{
recoverable_error_happened_ = true;
return CallbackReturn::SUCCESS;
}
else
{
return CallbackReturn::ERROR;
}
return CallbackReturn::FAILURE;
}
private:
double voltage_level_ = std::numeric_limits<double>::quiet_NaN( );
double voltage_level_hw_value_ = 0x666;
// Helper variables to initiate error on read
int read_calls_ = 0;
bool recoverable_error_happened_ = false;
};
class DummySystem : public hardware_interface::SystemInterface
{
CallbackReturn on_init( const hardware_interface::HardwareInfo & /* info */ ) override
{
// We hardcode the info
return CallbackReturn::SUCCESS;
}
CallbackReturn on_configure( const rclcpp_lifecycle::State & /*previous_state*/ ) override
{
for ( auto i = 0ul; i < 3; ++i )
{
position_state_[i] = 0.0;
velocity_state_[i] = 0.0;
}
// reset command only if error is initiated
if ( recoverable_error_happened_ )
{
for ( auto i = 0ul; i < 3; ++i )
{
velocity_command_[i] = 0.0;
}
}
read_calls_ = 0;
write_calls_ = 0;
return CallbackReturn::SUCCESS;
}
std::vector<hardware_interface::StateInterface> export_state_interfaces( ) override
{
// We can read a position and a velocity
std::vector<hardware_interface::StateInterface> state_interfaces;
state_interfaces.emplace_back( hardware_interface::StateInterface(
"joint1", hardware_interface::HW_IF_POSITION, &position_state_[0] ) );
state_interfaces.emplace_back( hardware_interface::StateInterface(
"joint1", hardware_interface::HW_IF_VELOCITY, &velocity_state_[0] ) );
state_interfaces.emplace_back( hardware_interface::StateInterface(
"joint2", hardware_interface::HW_IF_POSITION, &position_state_[1] ) );
state_interfaces.emplace_back( hardware_interface::StateInterface(
"joint2", hardware_interface::HW_IF_VELOCITY, &velocity_state_[1] ) );
state_interfaces.emplace_back( hardware_interface::StateInterface(
"joint3", hardware_interface::HW_IF_POSITION, &position_state_[2] ) );
state_interfaces.emplace_back( hardware_interface::StateInterface(
"joint3", hardware_interface::HW_IF_VELOCITY, &velocity_state_[2] ) );
return state_interfaces;
}
std::vector<hardware_interface::CommandInterface> export_command_interfaces( ) override
{
// We can command in velocity
std::vector<hardware_interface::CommandInterface> command_interfaces;
command_interfaces.emplace_back( hardware_interface::CommandInterface(
"joint1", hardware_interface::HW_IF_VELOCITY, &velocity_command_[0] ) );
command_interfaces.emplace_back( hardware_interface::CommandInterface(
"joint2", hardware_interface::HW_IF_VELOCITY, &velocity_command_[1] ) );
command_interfaces.emplace_back( hardware_interface::CommandInterface(
"joint3", hardware_interface::HW_IF_VELOCITY, &velocity_command_[2] ) );
return command_interfaces;
}
std::string get_name( ) const override { return "DummySystem"; }
hardware_interface::return_type read(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) override
{
++read_calls_;
if ( read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS )
{
return hardware_interface::return_type::ERROR;
}
// no-op, state is getting propagated within write.
return hardware_interface::return_type::OK;
}
hardware_interface::return_type write(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) override
{
++write_calls_;
if ( write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS )
{
return hardware_interface::return_type::ERROR;
}
for ( auto i = 0; i < 3; ++i )
{
position_state_[i] += velocity_command_[0];
velocity_state_[i] = velocity_command_[0];
}
return hardware_interface::return_type::OK;
}
CallbackReturn on_shutdown( const rclcpp_lifecycle::State & /*previous_state*/ ) override
{
for ( auto i = 0ul; i < 3; ++i )
{
velocity_state_[i] = 0.0;
}
return CallbackReturn::SUCCESS;
}
CallbackReturn on_error( const rclcpp_lifecycle::State & /*previous_state*/ ) override
{
if ( !recoverable_error_happened_ )
{
recoverable_error_happened_ = true;
return CallbackReturn::SUCCESS;
}
else
{
return CallbackReturn::ERROR;
}
return CallbackReturn::FAILURE;
}
private:
std::array<double, 3> position_state_ = {
std::numeric_limits<double>::quiet_NaN( ), std::numeric_limits<double>::quiet_NaN( ),
std::numeric_limits<double>::quiet_NaN( )};
std::array<double, 3> velocity_state_ = {
std::numeric_limits<double>::quiet_NaN( ), std::numeric_limits<double>::quiet_NaN( ),
std::numeric_limits<double>::quiet_NaN( )};
std::array<double, 3> velocity_command_ = {0.0, 0.0, 0.0};
// Helper variables to initiate error on read
unsigned int read_calls_ = 0;
unsigned int write_calls_ = 0;
bool recoverable_error_happened_ = false;
};
class DummySystemPreparePerform : public hardware_interface::SystemInterface
{
// Override the pure virtual functions with default behavior
CallbackReturn on_init( const hardware_interface::HardwareInfo & /* info */ ) override
{
// We hardcode the info
return CallbackReturn::SUCCESS;
}
std::vector<hardware_interface::StateInterface> export_state_interfaces( ) override { return {}; }
std::vector<hardware_interface::CommandInterface> export_command_interfaces( ) override
{
return {};
}
std::string get_name( ) const override { return "DummySystemPreparePerform"; }
hardware_interface::return_type read(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) override
{
return hardware_interface::return_type::OK;
}
hardware_interface::return_type write(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) override
{
return hardware_interface::return_type::OK;
}
// Custom prepare/perform functions
hardware_interface::return_type prepare_command_mode_switch(
const std::vector<std::string> & start_interfaces,
const std::vector<std::string> & stop_interfaces ) override
{
// Criteria to test against
if ( start_interfaces.size( ) != 1 )
{
return hardware_interface::return_type::ERROR;
}
if ( stop_interfaces.size( ) != 2 )
{
return hardware_interface::return_type::ERROR;
}
return hardware_interface::return_type::OK;
}
hardware_interface::return_type perform_command_mode_switch(
const std::vector<std::string> & start_interfaces,
const std::vector<std::string> & stop_interfaces ) override
{
// Criteria to test against
if ( start_interfaces.size( ) != 1 )
{
return hardware_interface::return_type::ERROR;
}
if ( stop_interfaces.size( ) != 2 )
{
return hardware_interface::return_type::ERROR;
}
return hardware_interface::return_type::OK;
}
};
} // namespace test_components
TEST( TestComponentInterfaces, dummy_actuator )
{
hardware_interface::Actuator actuator_hw( std::make_unique<test_components::DummyActuator>( ) );
hardware_interface::HardwareInfo mock_hw_info{};
auto state = actuator_hw.initialize( mock_hw_info );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label( ) );
auto state_interfaces = actuator_hw.export_state_interfaces( );
ASSERT_EQ( 2u, state_interfaces.size( ) );
EXPECT_EQ( "joint1/position", state_interfaces[0].get_name( ) );
EXPECT_EQ( hardware_interface::HW_IF_POSITION, state_interfaces[0].get_interface_name( ) );
EXPECT_EQ( "joint1", state_interfaces[0].get_prefix_name( ) );
EXPECT_EQ( "joint1/velocity", state_interfaces[1].get_name( ) );
EXPECT_EQ( hardware_interface::HW_IF_VELOCITY, state_interfaces[1].get_interface_name( ) );
EXPECT_EQ( "joint1", state_interfaces[1].get_prefix_name( ) );
auto command_interfaces = actuator_hw.export_command_interfaces( );
ASSERT_EQ( 1u, command_interfaces.size( ) );
EXPECT_EQ( "joint1/velocity", command_interfaces[0].get_name( ) );
EXPECT_EQ( hardware_interface::HW_IF_VELOCITY, command_interfaces[0].get_interface_name( ) );
EXPECT_EQ( "joint1", command_interfaces[0].get_prefix_name( ) );
double velocity_value = 1.0;
command_interfaces[0].set_value( velocity_value ); // velocity
ASSERT_EQ( hardware_interface::return_type::ERROR, actuator_hw.write( TIME, PERIOD ) );
// Noting should change because it is UNCONFIGURED
for ( auto step = 0u; step < 10; ++step )
{
ASSERT_EQ( hardware_interface::return_type::ERROR, actuator_hw.read( TIME, PERIOD ) );
ASSERT_TRUE( std::isnan( state_interfaces[0].get_value( ) ) ); // position value
ASSERT_TRUE( std::isnan( state_interfaces[1].get_value( ) ) ); // velocity
ASSERT_EQ( hardware_interface::return_type::ERROR, actuator_hw.write( TIME, PERIOD ) );
}
state = actuator_hw.configure( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::INACTIVE, state.label( ) );
// Read and Write are working because it is INACTIVE
for ( auto step = 0u; step < 10; ++step )
{
ASSERT_EQ( hardware_interface::return_type::OK, actuator_hw.read( TIME, PERIOD ) );
EXPECT_EQ( step * velocity_value, state_interfaces[0].get_value( ) ); // position value
EXPECT_EQ( step ? velocity_value : 0, state_interfaces[1].get_value( ) ); // velocity
ASSERT_EQ( hardware_interface::return_type::OK, actuator_hw.write( TIME, PERIOD ) );
}
state = actuator_hw.activate( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::ACTIVE, state.label( ) );
// Read and Write are working because it is ACTIVE
for ( auto step = 0u; step < 10; ++step )
{
ASSERT_EQ( hardware_interface::return_type::OK, actuator_hw.read( TIME, PERIOD ) );
EXPECT_EQ( ( 10 + step ) * velocity_value, state_interfaces[0].get_value( ) ); // position value
EXPECT_EQ( velocity_value, state_interfaces[1].get_value( ) ); // velocity
ASSERT_EQ( hardware_interface::return_type::OK, actuator_hw.write( TIME, PERIOD ) );
}
state = actuator_hw.shutdown( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::FINALIZED, state.label( ) );
// Noting should change because it is FINALIZED
for ( auto step = 0u; step < 10; ++step )
{
ASSERT_EQ( hardware_interface::return_type::ERROR, actuator_hw.read( TIME, PERIOD ) );
EXPECT_EQ( 20 * velocity_value, state_interfaces[0].get_value( ) ); // position value
EXPECT_EQ( 0, state_interfaces[1].get_value( ) ); // velocity
ASSERT_EQ( hardware_interface::return_type::ERROR, actuator_hw.write( TIME, PERIOD ) );
}
EXPECT_EQ(
hardware_interface::return_type::OK, actuator_hw.prepare_command_mode_switch( {""}, {""} ) );
EXPECT_EQ(
hardware_interface::return_type::OK, actuator_hw.perform_command_mode_switch( {""}, {""} ) );
}
TEST( TestComponentInterfaces, dummy_sensor )
{
hardware_interface::Sensor sensor_hw( std::make_unique<test_components::DummySensor>( ) );
hardware_interface::HardwareInfo mock_hw_info{};
auto state = sensor_hw.initialize( mock_hw_info );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label( ) );
auto state_interfaces = sensor_hw.export_state_interfaces( );
ASSERT_EQ( 1u, state_interfaces.size( ) );
EXPECT_EQ( "joint1/voltage", state_interfaces[0].get_name( ) );
EXPECT_EQ( "voltage", state_interfaces[0].get_interface_name( ) );
EXPECT_EQ( "joint1", state_interfaces[0].get_prefix_name( ) );
EXPECT_TRUE( std::isnan( state_interfaces[0].get_value( ) ) );
// Not updated because is is UNCONFIGURED
sensor_hw.read( TIME, PERIOD );
EXPECT_TRUE( std::isnan( state_interfaces[0].get_value( ) ) );
// Updated because is is INACTIVE
state = sensor_hw.configure( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::INACTIVE, state.label( ) );
EXPECT_EQ( 0.0, state_interfaces[0].get_value( ) );
// It can read now
sensor_hw.read( TIME, PERIOD );
EXPECT_EQ( 0x666, state_interfaces[0].get_value( ) );
}
TEST( TestComponentInterfaces, dummy_system )
{
hardware_interface::System system_hw( std::make_unique<test_components::DummySystem>( ) );
hardware_interface::HardwareInfo mock_hw_info{};
auto state = system_hw.initialize( mock_hw_info );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label( ) );
auto state_interfaces = system_hw.export_state_interfaces( );
ASSERT_EQ( 6u, state_interfaces.size( ) );
EXPECT_EQ( "joint1/position", state_interfaces[0].get_name( ) );
EXPECT_EQ( hardware_interface::HW_IF_POSITION, state_interfaces[0].get_interface_name( ) );
EXPECT_EQ( "joint1", state_interfaces[0].get_prefix_name( ) );
EXPECT_EQ( "joint1/velocity", state_interfaces[1].get_name( ) );
EXPECT_EQ( hardware_interface::HW_IF_VELOCITY, state_interfaces[1].get_interface_name( ) );
EXPECT_EQ( "joint1", state_interfaces[1].get_prefix_name( ) );
EXPECT_EQ( "joint2/position", state_interfaces[2].get_name( ) );
EXPECT_EQ( hardware_interface::HW_IF_POSITION, state_interfaces[2].get_interface_name( ) );
EXPECT_EQ( "joint2", state_interfaces[2].get_prefix_name( ) );
EXPECT_EQ( "joint2/velocity", state_interfaces[3].get_name( ) );
EXPECT_EQ( hardware_interface::HW_IF_VELOCITY, state_interfaces[3].get_interface_name( ) );
EXPECT_EQ( "joint2", state_interfaces[3].get_prefix_name( ) );
EXPECT_EQ( "joint3/position", state_interfaces[4].get_name( ) );
EXPECT_EQ( hardware_interface::HW_IF_POSITION, state_interfaces[4].get_interface_name( ) );
EXPECT_EQ( "joint3", state_interfaces[4].get_prefix_name( ) );
EXPECT_EQ( "joint3/velocity", state_interfaces[5].get_name( ) );
EXPECT_EQ( hardware_interface::HW_IF_VELOCITY, state_interfaces[5].get_interface_name( ) );
EXPECT_EQ( "joint3", state_interfaces[5].get_prefix_name( ) );
auto command_interfaces = system_hw.export_command_interfaces( );
ASSERT_EQ( 3u, command_interfaces.size( ) );
EXPECT_EQ( "joint1/velocity", command_interfaces[0].get_name( ) );
EXPECT_EQ( hardware_interface::HW_IF_VELOCITY, command_interfaces[0].get_interface_name( ) );
EXPECT_EQ( "joint1", command_interfaces[0].get_prefix_name( ) );
EXPECT_EQ( "joint2/velocity", command_interfaces[1].get_name( ) );
EXPECT_EQ( hardware_interface::HW_IF_VELOCITY, command_interfaces[1].get_interface_name( ) );
EXPECT_EQ( "joint2", command_interfaces[1].get_prefix_name( ) );
EXPECT_EQ( "joint3/velocity", command_interfaces[2].get_name( ) );
EXPECT_EQ( hardware_interface::HW_IF_VELOCITY, command_interfaces[2].get_interface_name( ) );
EXPECT_EQ( "joint3", command_interfaces[2].get_prefix_name( ) );
double velocity_value = 1.0;
command_interfaces[0].set_value( velocity_value ); // velocity
command_interfaces[1].set_value( velocity_value ); // velocity
command_interfaces[2].set_value( velocity_value ); // velocity
ASSERT_EQ( hardware_interface::return_type::ERROR, system_hw.write( TIME, PERIOD ) );
// Noting should change because it is UNCONFIGURED
for ( auto step = 0u; step < 10; ++step )
{
ASSERT_EQ( hardware_interface::return_type::ERROR, system_hw.read( TIME, PERIOD ) );
ASSERT_TRUE( std::isnan( state_interfaces[0].get_value( ) ) ); // position value
ASSERT_TRUE( std::isnan( state_interfaces[1].get_value( ) ) ); // velocity
ASSERT_TRUE( std::isnan( state_interfaces[2].get_value( ) ) ); // position value
ASSERT_TRUE( std::isnan( state_interfaces[3].get_value( ) ) ); // velocity
ASSERT_TRUE( std::isnan( state_interfaces[4].get_value( ) ) ); // position value
ASSERT_TRUE( std::isnan( state_interfaces[5].get_value( ) ) ); // velocity
ASSERT_EQ( hardware_interface::return_type::ERROR, system_hw.write( TIME, PERIOD ) );
}
state = system_hw.configure( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::INACTIVE, state.label( ) );
// Read and Write are working because it is INACTIVE
for ( auto step = 0u; step < 10; ++step )
{
ASSERT_EQ( hardware_interface::return_type::OK, system_hw.read( TIME, PERIOD ) );
EXPECT_EQ( step * velocity_value, state_interfaces[0].get_value( ) ); // position value
EXPECT_EQ( step ? velocity_value : 0, state_interfaces[1].get_value( ) ); // velocity
EXPECT_EQ( step * velocity_value, state_interfaces[2].get_value( ) ); // position value
EXPECT_EQ( step ? velocity_value : 0, state_interfaces[3].get_value( ) ); // velocity
EXPECT_EQ( step * velocity_value, state_interfaces[4].get_value( ) ); // position value
EXPECT_EQ( step ? velocity_value : 0, state_interfaces[5].get_value( ) ); // velocity
ASSERT_EQ( hardware_interface::return_type::OK, system_hw.write( TIME, PERIOD ) );
}
state = system_hw.activate( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::ACTIVE, state.label( ) );
// Read and Write are working because it is ACTIVE
for ( auto step = 0u; step < 10; ++step )
{
ASSERT_EQ( hardware_interface::return_type::OK, system_hw.read( TIME, PERIOD ) );
EXPECT_EQ( ( 10 + step ) * velocity_value, state_interfaces[0].get_value( ) ); // position value
EXPECT_EQ( velocity_value, state_interfaces[1].get_value( ) ); // velocity
EXPECT_EQ( ( 10 + step ) * velocity_value, state_interfaces[2].get_value( ) ); // position value
EXPECT_EQ( velocity_value, state_interfaces[3].get_value( ) ); // velocity
EXPECT_EQ( ( 10 + step ) * velocity_value, state_interfaces[4].get_value( ) ); // position value
EXPECT_EQ( velocity_value, state_interfaces[5].get_value( ) ); // velocity
ASSERT_EQ( hardware_interface::return_type::OK, system_hw.write( TIME, PERIOD ) );
}
state = system_hw.shutdown( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::FINALIZED, state.label( ) );
// Noting should change because it is FINALIZED
for ( auto step = 0u; step < 10; ++step )
{
ASSERT_EQ( hardware_interface::return_type::ERROR, system_hw.read( TIME, PERIOD ) );
EXPECT_EQ( 20 * velocity_value, state_interfaces[0].get_value( ) ); // position value
EXPECT_EQ( 0.0, state_interfaces[1].get_value( ) ); // velocity
EXPECT_EQ( 20 * velocity_value, state_interfaces[2].get_value( ) ); // position value
EXPECT_EQ( 0.0, state_interfaces[3].get_value( ) ); // velocity
EXPECT_EQ( 20 * velocity_value, state_interfaces[4].get_value( ) ); // position value
EXPECT_EQ( 0.0, state_interfaces[5].get_value( ) ); // velocity
ASSERT_EQ( hardware_interface::return_type::ERROR, system_hw.write( TIME, PERIOD ) );
}
EXPECT_EQ( hardware_interface::return_type::OK, system_hw.prepare_command_mode_switch( {}, {} ) );
EXPECT_EQ( hardware_interface::return_type::OK, system_hw.perform_command_mode_switch( {}, {} ) );
}
TEST( TestComponentInterfaces, dummy_command_mode_system )
{
hardware_interface::System system_hw(
std::make_unique<test_components::DummySystemPreparePerform>( ) );
hardware_interface::HardwareInfo mock_hw_info{};
auto state = system_hw.initialize( mock_hw_info );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label( ) );
std::vector<std::string> one_key = {"joint1/position"};
std::vector<std::string> two_keys = {"joint1/position", "joint1/velocity"};
// Only calls with ( one_key, two_keys ) should return OK
EXPECT_EQ(
hardware_interface::return_type::ERROR,
system_hw.prepare_command_mode_switch( one_key, one_key ) );
EXPECT_EQ(
hardware_interface::return_type::ERROR,
system_hw.perform_command_mode_switch( one_key, one_key ) );
EXPECT_EQ(
hardware_interface::return_type::OK, system_hw.prepare_command_mode_switch( one_key, two_keys ) );
EXPECT_EQ(
hardware_interface::return_type::OK, system_hw.perform_command_mode_switch( one_key, two_keys ) );
EXPECT_EQ(
hardware_interface::return_type::ERROR,
system_hw.prepare_command_mode_switch( two_keys, one_key ) );
EXPECT_EQ(
hardware_interface::return_type::ERROR,
system_hw.perform_command_mode_switch( two_keys, one_key ) );
}
TEST( TestComponentInterfaces, dummy_actuator_read_error_behavior )
{
hardware_interface::Actuator actuator_hw( std::make_unique<test_components::DummyActuator>( ) );
hardware_interface::HardwareInfo mock_hw_info{};
auto state = actuator_hw.initialize( mock_hw_info );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label( ) );
auto state_interfaces = actuator_hw.export_state_interfaces( );
auto command_interfaces = actuator_hw.export_command_interfaces( );
state = actuator_hw.configure( );
state = actuator_hw.activate( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::ACTIVE, state.label( ) );
ASSERT_EQ( hardware_interface::return_type::OK, actuator_hw.read( TIME, PERIOD ) );
ASSERT_EQ( hardware_interface::return_type::OK, actuator_hw.write( TIME, PERIOD ) );
// Initiate error on write ( this is first time therefore recoverable )
for ( auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i )
{
ASSERT_EQ( hardware_interface::return_type::OK, actuator_hw.read( TIME, PERIOD ) );
}
ASSERT_EQ( hardware_interface::return_type::ERROR, actuator_hw.read( TIME, PERIOD ) );
state = actuator_hw.get_state( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label( ) );
// activate again and expect reset values
state = actuator_hw.configure( );
EXPECT_EQ( state_interfaces[0].get_value( ), 0.0 );
EXPECT_EQ( command_interfaces[0].get_value( ), 0.0 );
state = actuator_hw.activate( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::ACTIVE, state.label( ) );
ASSERT_EQ( hardware_interface::return_type::OK, actuator_hw.read( TIME, PERIOD ) );
ASSERT_EQ( hardware_interface::return_type::OK, actuator_hw.write( TIME, PERIOD ) );
// Initiate error on write ( this is the second time therefore unrecoverable )
for ( auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i )
{
ASSERT_EQ( hardware_interface::return_type::OK, actuator_hw.read( TIME, PERIOD ) );
}
ASSERT_EQ( hardware_interface::return_type::ERROR, actuator_hw.read( TIME, PERIOD ) );
state = actuator_hw.get_state( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::FINALIZED, state.label( ) );
// can not change state anymore
state = actuator_hw.configure( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::FINALIZED, state.label( ) );
}
TEST( TestComponentInterfaces, dummy_actuator_write_error_behavior )
{
hardware_interface::Actuator actuator_hw( std::make_unique<test_components::DummyActuator>( ) );
hardware_interface::HardwareInfo mock_hw_info{};
auto state = actuator_hw.initialize( mock_hw_info );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label( ) );
auto state_interfaces = actuator_hw.export_state_interfaces( );
auto command_interfaces = actuator_hw.export_command_interfaces( );
state = actuator_hw.configure( );
state = actuator_hw.activate( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::ACTIVE, state.label( ) );
ASSERT_EQ( hardware_interface::return_type::OK, actuator_hw.read( TIME, PERIOD ) );
ASSERT_EQ( hardware_interface::return_type::OK, actuator_hw.write( TIME, PERIOD ) );
// Initiate error on write ( this is first time therefore recoverable )
for ( auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i )
{
ASSERT_EQ( hardware_interface::return_type::OK, actuator_hw.write( TIME, PERIOD ) );
}
ASSERT_EQ( hardware_interface::return_type::ERROR, actuator_hw.write( TIME, PERIOD ) );
state = actuator_hw.get_state( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label( ) );
// activate again and expect reset values
state = actuator_hw.configure( );
EXPECT_EQ( state_interfaces[0].get_value( ), 0.0 );
EXPECT_EQ( command_interfaces[0].get_value( ), 0.0 );
state = actuator_hw.activate( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::ACTIVE, state.label( ) );
ASSERT_EQ( hardware_interface::return_type::OK, actuator_hw.read( TIME, PERIOD ) );
ASSERT_EQ( hardware_interface::return_type::OK, actuator_hw.write( TIME, PERIOD ) );
// Initiate error on write ( this is the second time therefore unrecoverable )
for ( auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i )
{
ASSERT_EQ( hardware_interface::return_type::OK, actuator_hw.write( TIME, PERIOD ) );
}
ASSERT_EQ( hardware_interface::return_type::ERROR, actuator_hw.write( TIME, PERIOD ) );
state = actuator_hw.get_state( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::FINALIZED, state.label( ) );
// can not change state anymore
state = actuator_hw.configure( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::FINALIZED, state.label( ) );
}
TEST( TestComponentInterfaces, dummy_sensor_read_error_behavior )
{
hardware_interface::Sensor sensor_hw( std::make_unique<test_components::DummySensor>( ) );
hardware_interface::HardwareInfo mock_hw_info{};
auto state = sensor_hw.initialize( mock_hw_info );
auto state_interfaces = sensor_hw.export_state_interfaces( );
// Updated because is is INACTIVE
state = sensor_hw.configure( );
state = sensor_hw.activate( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::ACTIVE, state.label( ) );
ASSERT_EQ( hardware_interface::return_type::OK, sensor_hw.read( TIME, PERIOD ) );
// Initiate recoverable error - call read 99 times OK and on 100-time will return error
for ( auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i )
{
ASSERT_EQ( hardware_interface::return_type::OK, sensor_hw.read( TIME, PERIOD ) );
}
ASSERT_EQ( hardware_interface::return_type::ERROR, sensor_hw.read( TIME, PERIOD ) );
state = sensor_hw.get_state( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label( ) );
// activate again and expect reset values
state = sensor_hw.configure( );
EXPECT_EQ( state_interfaces[0].get_value( ), 0.0 );
state = sensor_hw.activate( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::ACTIVE, state.label( ) );
// Initiate unrecoverable error - call read 99 times OK and on 100-time will return error
for ( auto i = 1ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i )
{
ASSERT_EQ( hardware_interface::return_type::OK, sensor_hw.read( TIME, PERIOD ) );
}
ASSERT_EQ( hardware_interface::return_type::ERROR, sensor_hw.read( TIME, PERIOD ) );
state = sensor_hw.get_state( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::FINALIZED, state.label( ) );
// can not change state anymore
state = sensor_hw.configure( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::FINALIZED, state.label( ) );
}
TEST( TestComponentInterfaces, dummy_system_read_error_behavior )
{
hardware_interface::System system_hw( std::make_unique<test_components::DummySystem>( ) );
hardware_interface::HardwareInfo mock_hw_info{};
auto state = system_hw.initialize( mock_hw_info );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label( ) );
auto state_interfaces = system_hw.export_state_interfaces( );
auto command_interfaces = system_hw.export_command_interfaces( );
state = system_hw.configure( );
state = system_hw.activate( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::ACTIVE, state.label( ) );
ASSERT_EQ( hardware_interface::return_type::OK, system_hw.read( TIME, PERIOD ) );
ASSERT_EQ( hardware_interface::return_type::OK, system_hw.write( TIME, PERIOD ) );
// Initiate error on write ( this is first time therefore recoverable )
for ( auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i )
{
ASSERT_EQ( hardware_interface::return_type::OK, system_hw.read( TIME, PERIOD ) );
}
ASSERT_EQ( hardware_interface::return_type::ERROR, system_hw.read( TIME, PERIOD ) );
state = system_hw.get_state( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label( ) );
// activate again and expect reset values
state = system_hw.configure( );
for ( auto index = 0ul; index < 6; ++index )
{
EXPECT_EQ( state_interfaces[index].get_value( ), 0.0 );
}
for ( auto index = 0ul; index < 3; ++index )
{
EXPECT_EQ( command_interfaces[index].get_value( ), 0.0 );
}
state = system_hw.activate( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::ACTIVE, state.label( ) );
ASSERT_EQ( hardware_interface::return_type::OK, system_hw.read( TIME, PERIOD ) );
ASSERT_EQ( hardware_interface::return_type::OK, system_hw.write( TIME, PERIOD ) );
// Initiate error on write ( this is the second time therefore unrecoverable )
for ( auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i )
{
ASSERT_EQ( hardware_interface::return_type::OK, system_hw.read( TIME, PERIOD ) );
}
ASSERT_EQ( hardware_interface::return_type::ERROR, system_hw.read( TIME, PERIOD ) );
state = system_hw.get_state( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::FINALIZED, state.label( ) );
// can not change state anymore
state = system_hw.configure( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::FINALIZED, state.label( ) );
}
TEST( TestComponentInterfaces, dummy_system_write_error_behavior )
{
hardware_interface::System system_hw( std::make_unique<test_components::DummySystem>( ) );
hardware_interface::HardwareInfo mock_hw_info{};
auto state = system_hw.initialize( mock_hw_info );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label( ) );
auto state_interfaces = system_hw.export_state_interfaces( );
auto command_interfaces = system_hw.export_command_interfaces( );
state = system_hw.configure( );
state = system_hw.activate( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::ACTIVE, state.label( ) );
ASSERT_EQ( hardware_interface::return_type::OK, system_hw.read( TIME, PERIOD ) );
ASSERT_EQ( hardware_interface::return_type::OK, system_hw.write( TIME, PERIOD ) );
// Initiate error on write ( this is first time therefore recoverable )
for ( auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i )
{
ASSERT_EQ( hardware_interface::return_type::OK, system_hw.write( TIME, PERIOD ) );
}
ASSERT_EQ( hardware_interface::return_type::ERROR, system_hw.write( TIME, PERIOD ) );
state = system_hw.get_state( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::UNCONFIGURED, state.label( ) );
// activate again and expect reset values
state = system_hw.configure( );
for ( auto index = 0ul; index < 6; ++index )
{
EXPECT_EQ( state_interfaces[index].get_value( ), 0.0 );
}
for ( auto index = 0ul; index < 3; ++index )
{
EXPECT_EQ( command_interfaces[index].get_value( ), 0.0 );
}
state = system_hw.activate( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::ACTIVE, state.label( ) );
ASSERT_EQ( hardware_interface::return_type::OK, system_hw.read( TIME, PERIOD ) );
ASSERT_EQ( hardware_interface::return_type::OK, system_hw.write( TIME, PERIOD ) );
// Initiate error on write ( this is the second time therefore unrecoverable )
for ( auto i = 2ul; i < TRIGGER_READ_WRITE_ERROR_CALLS; ++i )
{
ASSERT_EQ( hardware_interface::return_type::OK, system_hw.write( TIME, PERIOD ) );
}
ASSERT_EQ( hardware_interface::return_type::ERROR, system_hw.write( TIME, PERIOD ) );
state = system_hw.get_state( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::FINALIZED, state.label( ) );
// can not change state anymore
state = system_hw.configure( );
EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, state.id( ) );
EXPECT_EQ( hardware_interface::lifecycle_state_names::FINALIZED, state.label( ) );
}
// Copyright 2020 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gmock/gmock.h>
#include <string>
#include "hardware_interface/component_parser.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "ros2_control_test_assets/components_urdfs.hpp"
#include "ros2_control_test_assets/descriptions.hpp"
#ifdef _WIN32
#define M_PI 3.1415926535897932384626433832795
#endif
using namespace ::testing; // NOLINT
29 class TestComponentParser : public Test
{
protected:
void SetUp( ) override {}
};
using hardware_interface::HW_IF_EFFORT;
using hardware_interface::HW_IF_POSITION;
using hardware_interface::HW_IF_VELOCITY;
using hardware_interface::parse_control_resources_from_urdf;
TEST_F( TestComponentParser, empty_string_throws_error )
{
ASSERT_THROW( parse_control_resources_from_urdf( "" ), std::runtime_error );
}
TEST_F( TestComponentParser, empty_urdf_throws_error )
{
const std::string empty_urdf =
"<?xml version=\"1.0\"?><robot name=\"robot\" xmlns=\"http://www.ros.org\"></robot>";
ASSERT_THROW( parse_control_resources_from_urdf( empty_urdf ), std::runtime_error );
}
TEST_F( TestComponentParser, string_robot_not_root_throws_error )
{
const std::string broken_xml_string =
R"(
<?xml version=\"1.0\"?><ros2_control name=\"robot\">><robot name=\"robot\" xmlns=\"http://www.ros.org\"></robot></ros2_control>
)";
ASSERT_THROW( parse_control_resources_from_urdf( broken_xml_string ), std::runtime_error );
}
TEST_F( TestComponentParser, invalid_child_throws_error )
{
const std::string broken_urdf_string =
std::string( ros2_control_test_assets::urdf_head ) +
ros2_control_test_assets::invalid_urdf_ros2_control_invalid_child +
ros2_control_test_assets::urdf_tail;
ASSERT_THROW( parse_control_resources_from_urdf( broken_urdf_string ), std::runtime_error );
}
TEST_F( TestComponentParser, missing_attribute_throws_error )
{
const std::string broken_urdf_string =
std::string( ros2_control_test_assets::urdf_head ) +
ros2_control_test_assets::invalid_urdf_ros2_control_missing_attribute +
ros2_control_test_assets::urdf_tail;
ASSERT_THROW( parse_control_resources_from_urdf( broken_urdf_string ), std::runtime_error );
}
TEST_F( TestComponentParser, parameter_missing_name_throws_error )
{
const std::string broken_urdf_string =
std::string( ros2_control_test_assets::urdf_head ) +
ros2_control_test_assets::invalid_urdf_ros2_control_parameter_missing_name +
ros2_control_test_assets::urdf_tail;
ASSERT_THROW( parse_control_resources_from_urdf( broken_urdf_string ), std::runtime_error );
}
TEST_F( TestComponentParser, component_interface_type_empty_throws_error )
{
const std::string broken_urdf_string =
std::string( ros2_control_test_assets::urdf_head ) +
ros2_control_test_assets::invalid_urdf_ros2_control_component_interface_type_empty +
ros2_control_test_assets::urdf_tail;
ASSERT_THROW( parse_control_resources_from_urdf( broken_urdf_string ), std::runtime_error );
}
TEST_F( TestComponentParser, parameter_empty_throws_error )
{
const std::string broken_urdf_string =
std::string( ros2_control_test_assets::urdf_head ) +
ros2_control_test_assets::invalid_urdf_ros2_control_parameter_empty +
ros2_control_test_assets::urdf_tail;
ASSERT_THROW( parse_control_resources_from_urdf( broken_urdf_string ), std::runtime_error );
}
TEST_F( TestComponentParser, successfully_parse_valid_urdf_system_one_interface )
{
std::string urdf_to_test =
std::string( ros2_control_test_assets::urdf_head ) +
ros2_control_test_assets::valid_urdf_ros2_control_system_one_interface +
ros2_control_test_assets::urdf_tail;
const auto control_hardware = parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( control_hardware, SizeIs( 1 ) );
const auto hardware_info = control_hardware.front( );
EXPECT_EQ( hardware_info.name, "RRBotSystemPositionOnly" );
EXPECT_EQ( hardware_info.type, "system" );
EXPECT_EQ(
hardware_info.hardware_class_type,
"ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware" );
ASSERT_THAT( hardware_info.hardware_parameters, SizeIs( 2 ) );
EXPECT_EQ( hardware_info.hardware_parameters.at( "example_param_write_for_sec" ), "2" );
ASSERT_THAT( hardware_info.joints, SizeIs( 2 ) );
EXPECT_EQ( hardware_info.joints[0].name, "joint1" );
EXPECT_EQ( hardware_info.joints[0].type, "joint" );
ASSERT_THAT( hardware_info.joints[0].command_interfaces, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.joints[0].command_interfaces[0].name, HW_IF_POSITION );
EXPECT_EQ( hardware_info.joints[0].command_interfaces[0].min, "-1" );
EXPECT_EQ( hardware_info.joints[0].command_interfaces[0].max, "1" );
ASSERT_THAT( hardware_info.joints[0].state_interfaces, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION );
EXPECT_EQ( hardware_info.joints[1].name, "joint2" );
EXPECT_EQ( hardware_info.joints[1].type, "joint" );
ASSERT_THAT( hardware_info.joints[1].command_interfaces, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.joints[1].command_interfaces[0].name, HW_IF_POSITION );
EXPECT_EQ( hardware_info.joints[1].command_interfaces[0].min, "-1" );
EXPECT_EQ( hardware_info.joints[1].command_interfaces[0].max, "1" );
ASSERT_THAT( hardware_info.joints[1].state_interfaces, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.joints[1].state_interfaces[0].name, HW_IF_POSITION );
}
TEST_F( TestComponentParser, successfully_parse_valid_urdf_system_multi_interface )
{
std::string urdf_to_test =
std::string( ros2_control_test_assets::urdf_head ) +
ros2_control_test_assets::valid_urdf_ros2_control_system_multi_interface +
ros2_control_test_assets::urdf_tail;
const auto control_hardware = parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( control_hardware, SizeIs( 1 ) );
const auto hardware_info = control_hardware.front( );
EXPECT_EQ( hardware_info.name, "RRBotSystemMultiInterface" );
EXPECT_EQ( hardware_info.type, "system" );
EXPECT_EQ(
hardware_info.hardware_class_type,
"ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware" );
ASSERT_THAT( hardware_info.hardware_parameters, SizeIs( 2 ) );
EXPECT_EQ( hardware_info.hardware_parameters.at( "example_param_write_for_sec" ), "2" );
EXPECT_EQ( hardware_info.hardware_parameters.at( "example_param_read_for_sec" ), "2" );
ASSERT_THAT( hardware_info.joints, SizeIs( 2 ) );
EXPECT_EQ( hardware_info.joints[0].name, "joint1" );
EXPECT_EQ( hardware_info.joints[0].type, "joint" );
ASSERT_THAT( hardware_info.joints[0].command_interfaces, SizeIs( 3 ) );
EXPECT_EQ( hardware_info.joints[0].command_interfaces[0].name, HW_IF_POSITION );
EXPECT_EQ( hardware_info.joints[0].command_interfaces[0].initial_value, "1.2" );
EXPECT_EQ( hardware_info.joints[0].command_interfaces[1].initial_value, "3.4" );
ASSERT_THAT( hardware_info.joints[0].state_interfaces, SizeIs( 3 ) );
EXPECT_EQ( hardware_info.joints[0].state_interfaces[1].name, HW_IF_VELOCITY );
EXPECT_EQ( hardware_info.joints[1].name, "joint2" );
EXPECT_EQ( hardware_info.joints[1].type, "joint" );
ASSERT_THAT( hardware_info.joints[1].command_interfaces, SizeIs( 1 ) );
ASSERT_THAT( hardware_info.joints[1].state_interfaces, SizeIs( 3 ) );
EXPECT_EQ( hardware_info.joints[1].state_interfaces[2].name, HW_IF_EFFORT );
}
TEST_F( TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sensor )
{
std::string urdf_to_test =
std::string( ros2_control_test_assets::urdf_head ) +
ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_sensor +
ros2_control_test_assets::urdf_tail;
const auto control_hardware = parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( control_hardware, SizeIs( 1 ) );
const auto hardware_info = control_hardware.front( );
EXPECT_EQ( hardware_info.name, "RRBotSystemWithSensor" );
EXPECT_EQ( hardware_info.type, "system" );
EXPECT_EQ(
hardware_info.hardware_class_type, "ros2_control_demo_hardware/RRBotSystemWithSensorHardware" );
ASSERT_THAT( hardware_info.hardware_parameters, SizeIs( 2 ) );
EXPECT_EQ( hardware_info.hardware_parameters.at( "example_param_write_for_sec" ), "2" );
ASSERT_THAT( hardware_info.joints, SizeIs( 2 ) );
EXPECT_EQ( hardware_info.joints[0].name, "joint1" );
EXPECT_EQ( hardware_info.joints[0].type, "joint" );
EXPECT_EQ( hardware_info.joints[1].name, "joint2" );
EXPECT_EQ( hardware_info.joints[1].type, "joint" );
ASSERT_THAT( hardware_info.sensors, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.sensors[0].name, "tcp_fts_sensor" );
EXPECT_EQ( hardware_info.sensors[0].type, "sensor" );
EXPECT_THAT( hardware_info.sensors[0].state_interfaces, SizeIs( 6 ) );
EXPECT_THAT( hardware_info.sensors[0].command_interfaces, IsEmpty( ) );
EXPECT_THAT( hardware_info.sensors[0].state_interfaces[0].name, "fx" );
EXPECT_THAT( hardware_info.sensors[0].state_interfaces[1].name, "fy" );
EXPECT_THAT( hardware_info.sensors[0].state_interfaces[2].name, "fz" );
EXPECT_THAT( hardware_info.sensors[0].state_interfaces[3].name, "tx" );
EXPECT_THAT( hardware_info.sensors[0].state_interfaces[4].name, "ty" );
EXPECT_THAT( hardware_info.sensors[0].state_interfaces[5].name, "tz" );
ASSERT_THAT( hardware_info.sensors[0].parameters, SizeIs( 3 ) );
EXPECT_EQ( hardware_info.sensors[0].parameters.at( "frame_id" ), "kuka_tcp" );
EXPECT_EQ( hardware_info.sensors[0].parameters.at( "lower_limits" ), "-100" );
EXPECT_EQ( hardware_info.sensors[0].parameters.at( "upper_limits" ), "100" );
}
TEST_F( TestComponentParser, successfully_parse_valid_urdf_system_robot_with_external_sensor )
{
std::string urdf_to_test =
std::string( ros2_control_test_assets::urdf_head ) +
ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_external_sensor +
ros2_control_test_assets::urdf_tail;
const auto control_hardware = parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( control_hardware, SizeIs( 2 ) );
auto hardware_info = control_hardware.at( 0 );
EXPECT_EQ( hardware_info.name, "RRBotSystemPositionOnlyWithExternalSensor" );
EXPECT_EQ( hardware_info.type, "system" );
EXPECT_EQ(
hardware_info.hardware_class_type,
"ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware" );
ASSERT_THAT( hardware_info.hardware_parameters, SizeIs( 2 ) );
EXPECT_EQ( hardware_info.hardware_parameters.at( "example_param_write_for_sec" ), "2" );
ASSERT_THAT( hardware_info.joints, SizeIs( 2 ) );
EXPECT_EQ( hardware_info.joints[0].name, "joint1" );
EXPECT_EQ( hardware_info.joints[0].type, "joint" );
EXPECT_EQ( hardware_info.joints[1].name, "joint2" );
EXPECT_EQ( hardware_info.joints[1].type, "joint" );
ASSERT_THAT( hardware_info.sensors, SizeIs( 0 ) );
hardware_info = control_hardware.at( 1 );
EXPECT_EQ( hardware_info.name, "RRBotForceTorqueSensor2D" );
EXPECT_EQ( hardware_info.type, "sensor" );
ASSERT_THAT( hardware_info.hardware_parameters, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.hardware_parameters.at( "example_param_read_for_sec" ), "0.43" );
ASSERT_THAT( hardware_info.sensors, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.sensors[0].name, "tcp_fts_sensor" );
EXPECT_EQ( hardware_info.sensors[0].type, "sensor" );
EXPECT_EQ( hardware_info.sensors[0].parameters.at( "frame_id" ), "kuka_tcp" );
}
TEST_F( TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot )
{
std::string urdf_to_test =
std::string( ros2_control_test_assets::urdf_head ) +
ros2_control_test_assets::valid_urdf_ros2_control_actuator_modular_robot +
ros2_control_test_assets::urdf_tail;
const auto control_hardware = parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( control_hardware, SizeIs( 2 ) );
auto hardware_info = control_hardware.at( 0 );
EXPECT_EQ( hardware_info.name, "RRBotModularJoint1" );
EXPECT_EQ( hardware_info.type, "actuator" );
EXPECT_EQ(
hardware_info.hardware_class_type, "ros2_control_demo_hardware/PositionActuatorHardware" );
ASSERT_THAT( hardware_info.hardware_parameters, SizeIs( 2 ) );
EXPECT_EQ( hardware_info.hardware_parameters.at( "example_param_write_for_sec" ), "1.23" );
ASSERT_THAT( hardware_info.joints, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.joints[0].name, "joint1" );
EXPECT_EQ( hardware_info.joints[0].type, "joint" );
hardware_info = control_hardware.at( 1 );
EXPECT_EQ( hardware_info.name, "RRBotModularJoint2" );
EXPECT_EQ( hardware_info.type, "actuator" );
EXPECT_EQ(
hardware_info.hardware_class_type, "ros2_control_demo_hardware/PositionActuatorHardware" );
ASSERT_THAT( hardware_info.hardware_parameters, SizeIs( 2 ) );
EXPECT_EQ( hardware_info.hardware_parameters.at( "example_param_read_for_sec" ), "3" );
ASSERT_THAT( hardware_info.joints, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.joints[0].name, "joint2" );
EXPECT_EQ( hardware_info.joints[0].type, "joint" );
}
TEST_F( TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot_with_sensors )
{
std::string urdf_to_test =
std::string( ros2_control_test_assets::urdf_head ) +
ros2_control_test_assets::valid_urdf_ros2_control_actuator_modular_robot_sensors +
ros2_control_test_assets::urdf_tail;
const auto control_hardware = parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( control_hardware, SizeIs( 4 ) );
auto hardware_info = control_hardware.at( 0 );
EXPECT_EQ( hardware_info.name, "RRBotModularJoint1" );
EXPECT_EQ( hardware_info.type, "actuator" );
EXPECT_EQ(
hardware_info.hardware_class_type, "ros2_control_demo_hardware/VelocityActuatorHardware" );
ASSERT_THAT( hardware_info.hardware_parameters, SizeIs( 2 ) );
EXPECT_EQ( hardware_info.hardware_parameters.at( "example_param_write_for_sec" ), "1.23" );
ASSERT_THAT( hardware_info.joints, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.joints[0].name, "joint1" );
EXPECT_EQ( hardware_info.joints[0].type, "joint" );
ASSERT_THAT( hardware_info.joints[0].command_interfaces, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.joints[0].command_interfaces[0].name, HW_IF_VELOCITY );
ASSERT_THAT( hardware_info.transmissions, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.transmissions[0].name, "transmission1" );
EXPECT_EQ( hardware_info.transmissions[0].type, "transmission_interface/SimpleTansmission" );
ASSERT_THAT( hardware_info.transmissions[0].joints, SizeIs( 1 ) );
EXPECT_THAT( hardware_info.transmissions[0].joints[0].name, "joint1" );
EXPECT_THAT(
hardware_info.transmissions[0].joints[0].mechanical_reduction, DoubleNear( 1024.0 / M_PI, 0.01 ) );
ASSERT_THAT( hardware_info.transmissions[0].actuators, SizeIs( 1 ) );
EXPECT_THAT( hardware_info.transmissions[0].actuators[0].name, "actuator1" );
hardware_info = control_hardware.at( 1 );
EXPECT_EQ( hardware_info.name, "RRBotModularJoint2" );
EXPECT_EQ( hardware_info.type, "actuator" );
EXPECT_EQ(
hardware_info.hardware_class_type, "ros2_control_demo_hardware/VelocityActuatorHardware" );
ASSERT_THAT( hardware_info.hardware_parameters, SizeIs( 2 ) );
EXPECT_EQ( hardware_info.hardware_parameters.at( "example_param_read_for_sec" ), "3" );
ASSERT_THAT( hardware_info.joints, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.joints[0].name, "joint2" );
EXPECT_EQ( hardware_info.joints[0].type, "joint" );
ASSERT_THAT( hardware_info.joints[0].command_interfaces, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.joints[0].command_interfaces[0].name, HW_IF_VELOCITY );
EXPECT_EQ( hardware_info.joints[0].command_interfaces[0].min, "-1" );
EXPECT_EQ( hardware_info.joints[0].command_interfaces[0].max, "1" );
hardware_info = control_hardware.at( 2 );
EXPECT_EQ( hardware_info.name, "RRBotModularPositionSensorJoint1" );
EXPECT_EQ( hardware_info.type, "sensor" );
EXPECT_EQ( hardware_info.hardware_class_type, "ros2_control_demo_hardware/PositionSensorHardware" );
ASSERT_THAT( hardware_info.hardware_parameters, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.hardware_parameters.at( "example_param_read_for_sec" ), "2" );
ASSERT_THAT( hardware_info.sensors, SizeIs( 0 ) );
ASSERT_THAT( hardware_info.joints, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.joints[0].name, "joint1" );
EXPECT_EQ( hardware_info.joints[0].type, "joint" );
ASSERT_THAT( hardware_info.joints[0].command_interfaces, IsEmpty( ) );
ASSERT_THAT( hardware_info.joints[0].state_interfaces, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION );
hardware_info = control_hardware.at( 3 );
EXPECT_EQ( hardware_info.name, "RRBotModularPositionSensorJoint2" );
EXPECT_EQ( hardware_info.type, "sensor" );
EXPECT_EQ( hardware_info.hardware_class_type, "ros2_control_demo_hardware/PositionSensorHardware" );
ASSERT_THAT( hardware_info.hardware_parameters, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.hardware_parameters.at( "example_param_read_for_sec" ), "2" );
ASSERT_THAT( hardware_info.sensors, SizeIs( 0 ) );
ASSERT_THAT( hardware_info.joints, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.joints[0].name, "joint2" );
EXPECT_EQ( hardware_info.joints[0].type, "joint" );
ASSERT_THAT( hardware_info.joints[0].command_interfaces, IsEmpty( ) );
ASSERT_THAT( hardware_info.joints[0].state_interfaces, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION );
}
TEST_F( TestComponentParser, successfully_parse_valid_urdf_system_multi_joints_transmission )
{
std::string urdf_to_test =
std::string( ros2_control_test_assets::urdf_head ) +
ros2_control_test_assets::valid_urdf_ros2_control_system_multi_joints_transmission +
ros2_control_test_assets::urdf_tail;
const auto control_hardware = parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( control_hardware, SizeIs( 1 ) );
auto hardware_info = control_hardware.at( 0 );
EXPECT_EQ( hardware_info.name, "RRBotModularWrist" );
EXPECT_EQ( hardware_info.type, "system" );
EXPECT_EQ(
hardware_info.hardware_class_type, "ros2_control_demo_hardware/ActuatorHardwareMultiDOF" );
ASSERT_THAT( hardware_info.hardware_parameters, SizeIs( 2 ) );
EXPECT_EQ( hardware_info.hardware_parameters.at( "example_param_write_for_sec" ), "1.23" );
ASSERT_THAT( hardware_info.joints, SizeIs( 2 ) );
EXPECT_EQ( hardware_info.joints[0].name, "joint1" );
EXPECT_EQ( hardware_info.joints[0].type, "joint" );
EXPECT_EQ( hardware_info.joints[1].name, "joint2" );
EXPECT_EQ( hardware_info.joints[1].type, "joint" );
ASSERT_THAT( hardware_info.transmissions, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.transmissions[0].name, "transmission1" );
EXPECT_EQ( hardware_info.transmissions[0].type, "transmission_interface/DifferentialTransmission" );
ASSERT_THAT( hardware_info.transmissions[0].joints, SizeIs( 2 ) );
EXPECT_EQ( hardware_info.transmissions[0].joints[0].name, "joint1" );
EXPECT_EQ( hardware_info.transmissions[0].joints[0].role, "joint1" );
EXPECT_EQ( hardware_info.transmissions[0].joints[0].mechanical_reduction, 10.0 );
EXPECT_EQ( hardware_info.transmissions[0].joints[0].offset, 0.5 );
EXPECT_EQ( hardware_info.transmissions[0].joints[1].name, "joint2" );
EXPECT_EQ( hardware_info.transmissions[0].joints[1].role, "joint2" );
EXPECT_EQ( hardware_info.transmissions[0].joints[1].mechanical_reduction, 50.0 );
EXPECT_EQ( hardware_info.transmissions[0].joints[1].offset, 0.0 );
ASSERT_THAT( hardware_info.transmissions[0].actuators, SizeIs( 2 ) );
EXPECT_EQ( hardware_info.transmissions[0].actuators[0].name, "joint1_motor" );
EXPECT_EQ( hardware_info.transmissions[0].actuators[0].role, "actuator1" );
EXPECT_EQ( hardware_info.transmissions[0].actuators[1].name, "joint2_motor" );
EXPECT_EQ( hardware_info.transmissions[0].actuators[1].role, "actuator2" );
}
TEST_F( TestComponentParser, successfully_parse_valid_urdf_sensor_only )
{
std::string urdf_to_test = std::string( ros2_control_test_assets::urdf_head ) +
ros2_control_test_assets::valid_urdf_ros2_control_sensor_only +
ros2_control_test_assets::urdf_tail;
const auto control_hardware = parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( control_hardware, SizeIs( 1 ) );
auto hardware_info = control_hardware.at( 0 );
EXPECT_EQ( hardware_info.name, "CameraWithIMU" );
EXPECT_EQ( hardware_info.type, "sensor" );
EXPECT_EQ( hardware_info.hardware_class_type, "ros2_control_demo_hardware/CameraWithIMUSensor" );
ASSERT_THAT( hardware_info.hardware_parameters, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.hardware_parameters.at( "example_param_read_for_sec" ), "2" );
ASSERT_THAT( hardware_info.sensors, SizeIs( 2 ) );
EXPECT_EQ( hardware_info.sensors[0].name, "sensor1" );
EXPECT_EQ( hardware_info.sensors[0].type, "sensor" );
ASSERT_THAT( hardware_info.sensors[0].state_interfaces, SizeIs( 3 ) );
EXPECT_EQ( hardware_info.sensors[0].state_interfaces[0].name, "roll" );
EXPECT_EQ( hardware_info.sensors[0].state_interfaces[1].name, "pitch" );
EXPECT_EQ( hardware_info.sensors[0].state_interfaces[2].name, "yaw" );
EXPECT_EQ( hardware_info.sensors[1].name, "sensor2" );
EXPECT_EQ( hardware_info.sensors[1].type, "sensor" );
ASSERT_THAT( hardware_info.sensors[1].state_interfaces, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.sensors[1].state_interfaces[0].name, "image" );
}
TEST_F( TestComponentParser, successfully_parse_valid_urdf_actuator_only )
{
std::string urdf_to_test = std::string( ros2_control_test_assets::urdf_head ) +
ros2_control_test_assets::valid_urdf_ros2_control_actuator_only +
ros2_control_test_assets::urdf_tail;
const auto control_hardware = parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( control_hardware, SizeIs( 1 ) );
auto hardware_info = control_hardware.at( 0 );
EXPECT_EQ( hardware_info.name, "ActuatorModularJoint1" );
EXPECT_EQ( hardware_info.type, "actuator" );
EXPECT_EQ(
hardware_info.hardware_class_type, "ros2_control_demo_hardware/VelocityActuatorHardware" );
ASSERT_THAT( hardware_info.hardware_parameters, SizeIs( 2 ) );
EXPECT_EQ( hardware_info.hardware_parameters.at( "example_param_write_for_sec" ), "1.13" );
ASSERT_THAT( hardware_info.joints, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.joints[0].name, "joint1" );
EXPECT_EQ( hardware_info.joints[0].type, "joint" );
ASSERT_THAT( hardware_info.joints[0].command_interfaces, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.joints[0].command_interfaces[0].name, HW_IF_VELOCITY );
EXPECT_EQ( hardware_info.joints[0].command_interfaces[0].min, "-1" );
EXPECT_EQ( hardware_info.joints[0].command_interfaces[0].max, "1" );
ASSERT_THAT( hardware_info.joints[0].state_interfaces, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.joints[0].state_interfaces[0].name, HW_IF_VELOCITY );
ASSERT_THAT( hardware_info.transmissions, SizeIs( 1 ) );
const auto transmission = hardware_info.transmissions[0];
EXPECT_EQ( transmission.name, "transmission1" );
EXPECT_EQ( transmission.type, "transmission_interface/RotationToLinerTansmission" );
EXPECT_THAT( transmission.joints, SizeIs( 1 ) );
const auto joint = transmission.joints[0];
EXPECT_EQ( joint.name, "joint1" );
EXPECT_EQ( joint.role, "joint1" );
EXPECT_THAT( joint.interfaces, ElementsAre( "velocity" ) );
EXPECT_THAT( joint.mechanical_reduction, DoubleEq( 325.949 ) );
EXPECT_THAT( joint.offset, DoubleEq( 0.0 ) );
EXPECT_THAT( transmission.actuators, SizeIs( 1 ) );
const auto actuator = transmission.actuators[0];
EXPECT_EQ( actuator.name, "actuator1" );
EXPECT_EQ( actuator.role, "actuator1" );
EXPECT_THAT( actuator.interfaces, ContainerEq( joint.interfaces ) );
EXPECT_THAT( actuator.offset, DoubleEq( 0.0 ) );
ASSERT_THAT( transmission.parameters, SizeIs( 1 ) );
EXPECT_EQ( transmission.parameters.at( "additional_special_parameter" ), "1337" );
}
TEST_F( TestComponentParser, successfully_parse_valid_urdf_system_robot_with_gpio )
{
std::string urdf_to_test =
std::string( ros2_control_test_assets::urdf_head ) +
ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_gpio +
ros2_control_test_assets::urdf_tail;
const auto control_hardware = parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( control_hardware, SizeIs( 1 ) );
auto hardware_info = control_hardware.front( );
EXPECT_EQ( hardware_info.name, "RRBotSystemWithGPIO" );
EXPECT_EQ( hardware_info.type, "system" );
EXPECT_EQ(
hardware_info.hardware_class_type, "ros2_control_demo_hardware/RRBotSystemWithGPIOHardware" );
ASSERT_THAT( hardware_info.joints, SizeIs( 2 ) );
EXPECT_EQ( hardware_info.joints[0].name, "joint1" );
EXPECT_EQ( hardware_info.joints[0].type, "joint" );
EXPECT_EQ( hardware_info.joints[1].name, "joint2" );
EXPECT_EQ( hardware_info.joints[1].type, "joint" );
ASSERT_THAT( hardware_info.gpios, SizeIs( 2 ) );
EXPECT_EQ( hardware_info.gpios[0].name, "flange_analog_IOs" );
EXPECT_EQ( hardware_info.gpios[0].type, "gpio" );
EXPECT_THAT( hardware_info.gpios[0].state_interfaces, SizeIs( 3 ) );
EXPECT_THAT( hardware_info.gpios[0].command_interfaces, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.gpios[0].state_interfaces[0].name, "analog_output1" );
EXPECT_EQ( hardware_info.gpios[0].state_interfaces[1].name, "analog_input1" );
EXPECT_EQ( hardware_info.gpios[0].state_interfaces[2].name, "analog_input2" );
EXPECT_EQ( hardware_info.gpios[1].name, "flange_vacuum" );
EXPECT_EQ( hardware_info.gpios[1].type, "gpio" );
EXPECT_THAT( hardware_info.gpios[1].state_interfaces, SizeIs( 1 ) );
EXPECT_THAT( hardware_info.gpios[1].command_interfaces, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.gpios[1].state_interfaces[0].name, "vacuum" );
EXPECT_EQ( hardware_info.gpios[1].command_interfaces[0].name, "vacuum" );
EXPECT_THAT( hardware_info.transmissions, IsEmpty( ) );
}
TEST_F( TestComponentParser, successfully_parse_valid_urdf_system_with_size_and_data_type )
{
std::string urdf_to_test =
std::string( ros2_control_test_assets::urdf_head ) +
ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_size_and_data_type +
ros2_control_test_assets::urdf_tail;
const auto control_hardware = parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( control_hardware, SizeIs( 1 ) );
auto hardware_info = control_hardware.front( );
EXPECT_EQ( hardware_info.name, "RRBotSystemWithSizeAndDataType" );
EXPECT_EQ( hardware_info.type, "system" );
EXPECT_EQ(
hardware_info.hardware_class_type, "ros2_control_demo_hardware/RRBotSystemWithSizeAndDataType" );
ASSERT_THAT( hardware_info.joints, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.joints[0].name, "joint1" );
EXPECT_EQ( hardware_info.joints[0].type, "joint" );
EXPECT_THAT( hardware_info.joints[0].command_interfaces, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.joints[0].command_interfaces[0].name, HW_IF_POSITION );
EXPECT_EQ( hardware_info.joints[0].command_interfaces[0].data_type, "double" );
EXPECT_EQ( hardware_info.joints[0].command_interfaces[0].size, 1 );
EXPECT_THAT( hardware_info.joints[0].state_interfaces, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION );
EXPECT_EQ( hardware_info.joints[0].state_interfaces[0].data_type, "double" );
EXPECT_EQ( hardware_info.joints[0].state_interfaces[0].size, 1 );
ASSERT_THAT( hardware_info.gpios, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.gpios[0].name, "flange_IOS" );
EXPECT_EQ( hardware_info.gpios[0].type, "gpio" );
EXPECT_THAT( hardware_info.gpios[0].command_interfaces, SizeIs( 1 ) );
EXPECT_EQ( hardware_info.gpios[0].command_interfaces[0].name, "digital_output" );
EXPECT_EQ( hardware_info.gpios[0].command_interfaces[0].data_type, "bool" );
EXPECT_EQ( hardware_info.gpios[0].command_interfaces[0].size, 2 );
EXPECT_THAT( hardware_info.gpios[0].state_interfaces, SizeIs( 2 ) );
EXPECT_EQ( hardware_info.gpios[0].state_interfaces[0].name, "analog_input" );
EXPECT_EQ( hardware_info.gpios[0].state_interfaces[0].data_type, "double" );
EXPECT_EQ( hardware_info.gpios[0].state_interfaces[0].size, 3 );
EXPECT_EQ( hardware_info.gpios[0].state_interfaces[1].name, "image" );
EXPECT_EQ( hardware_info.gpios[0].state_interfaces[1].data_type, "cv::Mat" );
EXPECT_EQ( hardware_info.gpios[0].state_interfaces[1].size, 1 );
}
TEST_F( TestComponentParser, negative_size_throws_error )
{
std::string urdf_to_test = std::string( ros2_control_test_assets::urdf_head ) +
ros2_control_test_assets::invalid_urdf2_ros2_control_illegal_size +
ros2_control_test_assets::urdf_tail;
ASSERT_THROW( parse_control_resources_from_urdf( urdf_to_test ), std::runtime_error );
}
TEST_F( TestComponentParser, noninteger_size_throws_error )
{
std::string urdf_to_test = std::string( ros2_control_test_assets::urdf_head ) +
ros2_control_test_assets::invalid_urdf2_ros2_control_illegal_size2 +
ros2_control_test_assets::urdf_tail;
ASSERT_THROW( parse_control_resources_from_urdf( urdf_to_test ), std::runtime_error );
}
TEST_F( TestComponentParser, transmission_and_component_joint_mismatch_throws_error )
{
std::string urdf_to_test =
std::string( ros2_control_test_assets::urdf_head ) +
ros2_control_test_assets::invalid_urdf2_hw_transmission_joint_mismatch +
ros2_control_test_assets::urdf_tail;
ASSERT_THROW( parse_control_resources_from_urdf( urdf_to_test ), std::runtime_error );
}
TEST_F( TestComponentParser, transmission_given_too_many_joints_throws_error )
{
std::string urdf_to_test =
std::string( ros2_control_test_assets::urdf_head ) +
ros2_control_test_assets::invalid_urdf2_transmission_given_too_many_joints +
ros2_control_test_assets::urdf_tail;
ASSERT_THROW( parse_control_resources_from_urdf( urdf_to_test ), std::runtime_error );
}
// Copyright 2020 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include <vector>
#include "hardware_interface/actuator_interface.hpp"
using hardware_interface::ActuatorInterface;
using hardware_interface::CommandInterface;
using hardware_interface::return_type;
using hardware_interface::StateInterface;
25 class TestActuator : public ActuatorInterface
{
CallbackReturn on_init( const hardware_interface::HardwareInfo & info ) override
{
if ( ActuatorInterface::on_init( info ) != CallbackReturn::SUCCESS )
{
return CallbackReturn::ERROR;
}
/*
* a hardware can optional prove for incorrect info here.
*
* // can only control one joint
* if ( info_.joints.size( ) != 1 ) {return CallbackReturn::ERROR;}
* // can only control in position
* if ( info_.joints[0].command_interfaces.size( ) != 1 ) {return CallbackReturn::ERROR;}
* // can only give feedback state for position and velocity
* if ( info_.joints[0].state_interfaces.size( ) != 2 ) {return CallbackReturn::ERROR;}
*/
return CallbackReturn::SUCCESS;
}
std::vector<StateInterface> export_state_interfaces( ) override
{
std::vector<StateInterface> state_interfaces;
state_interfaces.emplace_back( hardware_interface::StateInterface(
info_.joints[0].name, info_.joints[0].state_interfaces[0].name, &position_state_ ) );
state_interfaces.emplace_back( hardware_interface::StateInterface(
info_.joints[0].name, info_.joints[0].state_interfaces[1].name, &velocity_state_ ) );
state_interfaces.emplace_back(
hardware_interface::StateInterface( info_.joints[0].name, "some_unlisted_interface", nullptr ) );
return state_interfaces;
}
std::vector<CommandInterface> export_command_interfaces( ) override
{
std::vector<CommandInterface> command_interfaces;
command_interfaces.emplace_back( hardware_interface::CommandInterface(
info_.joints[0].name, info_.joints[0].command_interfaces[0].name, &velocity_command_ ) );
if ( info_.joints[0].command_interfaces.size( ) > 1 )
{
command_interfaces.emplace_back( hardware_interface::CommandInterface(
info_.joints[0].name, info_.joints[0].command_interfaces[1].name, &max_velocity_command_ ) );
}
return command_interfaces;
}
return_type read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) override
{
// The next line is for the testing purposes. We need value to be changed to be sure that
// the feedback from hardware to controllers in the chain is working as it should.
// This makes value checks clearer and confirms there is no "state = command" line or some
// other mixture of interfaces somewhere in the test stack.
velocity_state_ = velocity_command_ / 2;
return return_type::OK;
}
return_type write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) override
{
return return_type::OK;
}
private:
double position_state_ = 0.0;
double velocity_state_ = 0.0;
double velocity_command_ = 0.0;
double max_velocity_command_ = 0.0;
};
#include "pluginlib/class_list_macros.hpp" // NOLINT
PLUGINLIB_EXPORT_CLASS( TestActuator, hardware_interface::ActuatorInterface )
// Copyright 2020 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include <vector>
#include "hardware_interface/sensor_interface.hpp"
using hardware_interface::return_type;
using hardware_interface::SensorInterface;
using hardware_interface::StateInterface;
24 class TestSensor : public SensorInterface
{
CallbackReturn on_init( const hardware_interface::HardwareInfo & info ) override
{
if ( SensorInterface::on_init( info ) != CallbackReturn::SUCCESS )
{
return CallbackReturn::ERROR;
}
// can only give feedback state for velocity
if ( info_.sensors[0].state_interfaces.size( ) != 1 )
{
return CallbackReturn::ERROR;
}
return CallbackReturn::SUCCESS;
}
std::vector<StateInterface> export_state_interfaces( ) override
{
std::vector<StateInterface> state_interfaces;
state_interfaces.emplace_back( hardware_interface::StateInterface(
info_.sensors[0].name, info_.sensors[0].state_interfaces[0].name, &velocity_state_ ) );
return state_interfaces;
}
return_type read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) override
{
return return_type::OK;
}
private:
double velocity_state_ = 0.0;
};
#include "pluginlib/class_list_macros.hpp" // NOLINT
PLUGINLIB_EXPORT_CLASS( TestSensor, hardware_interface::SensorInterface )
// Copyright 2020 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <array>
#include <memory>
#include <vector>
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
using hardware_interface::CommandInterface;
using hardware_interface::return_type;
using hardware_interface::StateInterface;
using hardware_interface::SystemInterface;
27 class TestSystem : public SystemInterface
{
std::vector<StateInterface> export_state_interfaces( ) override
{
std::vector<StateInterface> state_interfaces;
for ( auto i = 0u; i < info_.joints.size( ); ++i )
{
if ( info_.joints[i].name != "configuration" )
{
state_interfaces.emplace_back( hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i] ) );
state_interfaces.emplace_back( hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i] ) );
state_interfaces.emplace_back( hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i] ) );
}
}
if ( info_.joints.size( ) > 2 )
{
// Add configuration/max_tcp_jerk interface
state_interfaces.emplace_back( hardware_interface::StateInterface(
info_.joints[2].name, info_.joints[2].state_interfaces[0].name, &configuration_state_ ) );
}
return state_interfaces;
}
std::vector<CommandInterface> export_command_interfaces( ) override
{
std::vector<CommandInterface> command_interfaces;
for ( auto i = 0u; i < info_.joints.size( ); ++i )
{
if ( info_.joints[i].name != "configuration" )
{
command_interfaces.emplace_back( hardware_interface::CommandInterface(
info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i] ) );
}
}
// Add max_acceleration command interface
command_interfaces.emplace_back( hardware_interface::CommandInterface(
info_.joints[0].name, info_.joints[0].command_interfaces[1].name,
&max_acceleration_command_ ) );
if ( info_.joints.size( ) > 2 )
{
// Add configuration/max_tcp_jerk interface
command_interfaces.emplace_back( hardware_interface::CommandInterface(
info_.joints[2].name, info_.joints[2].command_interfaces[0].name, &configuration_command_ ) );
}
return command_interfaces;
}
return_type read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) override
{
return return_type::OK;
}
return_type write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) override
{
return return_type::OK;
}
private:
std::array<double, 2> velocity_command_ = {0.0, 0.0};
std::array<double, 2> position_state_ = {0.0, 0.0};
std::array<double, 2> velocity_state_ = {0.0, 0.0};
std::array<double, 2> acceleration_state_ = {0.0, 0.0};
double max_acceleration_command_ = 0.0;
double configuration_state_ = 0.0;
double configuration_command_ = 0.0;
};
#include "pluginlib/class_list_macros.hpp" // NOLINT
PLUGINLIB_EXPORT_CLASS( TestSystem, hardware_interface::SystemInterface )
// Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gmock/gmock.h>
#include "hardware_interface/handle.hpp"
using hardware_interface::CommandInterface;
using hardware_interface::StateInterface;
namespace
{
23 constexpr auto JOINT_NAME = "joint_1";
constexpr auto FOO_INTERFACE = "FooInterface";
} // namespace
TEST( TestHandle, command_interface )
{
double value = 1.337;
CommandInterface interface{JOINT_NAME, FOO_INTERFACE, &value};
EXPECT_DOUBLE_EQ( interface.get_value( ), value );
EXPECT_NO_THROW( interface.set_value( 0.0 ) );
EXPECT_DOUBLE_EQ( interface.get_value( ), 0.0 );
}
TEST( TestHandle, state_interface )
{
double value = 1.337;
StateInterface interface{JOINT_NAME, FOO_INTERFACE, &value};
EXPECT_DOUBLE_EQ( interface.get_value( ), value );
// interface.set_value( 5 ); compiler error, no set_value function
}
TEST( TestHandle, name_getters_work )
{
StateInterface handle{JOINT_NAME, FOO_INTERFACE};
EXPECT_EQ( handle.get_name( ), std::string( JOINT_NAME ) + "/" + std::string( FOO_INTERFACE ) );
EXPECT_EQ( handle.get_interface_name( ), FOO_INTERFACE );
EXPECT_EQ( handle.get_prefix_name( ), JOINT_NAME );
}
TEST( TestHandle, value_methods_throw_for_nullptr )
{
CommandInterface handle{JOINT_NAME, FOO_INTERFACE};
EXPECT_ANY_THROW( handle.get_value( ) );
EXPECT_ANY_THROW( handle.set_value( 0.0 ) );
}
TEST( TestHandle, value_methods_work_on_non_nullptr )
{
double value = 1.337;
CommandInterface handle{JOINT_NAME, FOO_INTERFACE, &value};
EXPECT_DOUBLE_EQ( handle.get_value( ), value );
EXPECT_NO_THROW( handle.set_value( 0.0 ) );
EXPECT_DOUBLE_EQ( handle.get_value( ), 0.0 );
}
// Copyright 2020 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <algorithm>
#include <cmath>
#include <memory>
#include <vector>
#include "hardware_interface/sensor_interface.hpp"
using hardware_interface::return_type;
using hardware_interface::SensorInterface;
using hardware_interface::StateInterface;
namespace test_hardware_components
{
28 class TestForceTorqueSensor : public SensorInterface
{
CallbackReturn on_init( const hardware_interface::HardwareInfo & sensor_info ) override
{
if ( SensorInterface::on_init( sensor_info ) != CallbackReturn::SUCCESS )
{
return CallbackReturn::ERROR;
}
const auto & state_interfaces = info_.sensors[0].state_interfaces;
38 if ( state_interfaces.size( ) != 6 )
{
return CallbackReturn::ERROR;
}
for ( const auto & ft_key : {"fx", "fy", "fz", "tx", "ty", "tz"} )
{
if (
std::find_if(
state_interfaces.begin( ), state_interfaces.end( ),
[&ft_key]( const auto & interface_info )
{ return interface_info.name == ft_key; } ) == state_interfaces.end( ) )
{
return CallbackReturn::ERROR;
}
}
fprintf( stderr, "TestForceTorqueSensor configured successfully.\n" );
return CallbackReturn::SUCCESS;
}
std::vector<StateInterface> export_state_interfaces( ) override
{
std::vector<StateInterface> state_interfaces;
const auto & sensor_name = info_.sensors[0].name;
state_interfaces.emplace_back(
hardware_interface::StateInterface( sensor_name, "fx", &values_.fx ) );
state_interfaces.emplace_back(
hardware_interface::StateInterface( sensor_name, "fy", &values_.fy ) );
state_interfaces.emplace_back(
hardware_interface::StateInterface( sensor_name, "fz", &values_.fz ) );
state_interfaces.emplace_back(
hardware_interface::StateInterface( sensor_name, "tx", &values_.tx ) );
state_interfaces.emplace_back(
hardware_interface::StateInterface( sensor_name, "ty", &values_.ty ) );
state_interfaces.emplace_back(
hardware_interface::StateInterface( sensor_name, "tz", &values_.tz ) );
return state_interfaces;
}
return_type read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) override
{
values_.fx = fmod( ( values_.fx + 1.0 ), 10 );
values_.fy = fmod( ( values_.fy + 1.0 ), 10 );
values_.fz = fmod( ( values_.fz + 1.0 ), 10 );
values_.tx = fmod( ( values_.tx + 1.0 ), 10 );
values_.ty = fmod( ( values_.ty + 1.0 ), 10 );
values_.tz = fmod( ( values_.tz + 1.0 ), 10 );
return return_type::OK;
}
private:
struct FTValues
{
double fx = 0.0;
double fy = 0.0;
double fz = 0.0;
double tx = 0.0;
double ty = 0.0;
double tz = 0.0;
};
FTValues values_;
};
} // namespace test_hardware_components
#include "pluginlib/class_list_macros.hpp" // NOLINT
PLUGINLIB_EXPORT_CLASS(
test_hardware_components::TestForceTorqueSensor, hardware_interface::SensorInterface )
// Copyright 2020 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <memory>
#include <vector>
#include "hardware_interface/actuator_interface.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
using hardware_interface::ActuatorInterface;
using hardware_interface::CommandInterface;
using hardware_interface::return_type;
using hardware_interface::StateInterface;
namespace test_hardware_components
{
28 class TestSingleJointActuator : public ActuatorInterface
{
CallbackReturn on_init( const hardware_interface::HardwareInfo & actuator_info ) override
{
if ( ActuatorInterface::on_init( actuator_info ) != CallbackReturn::SUCCESS )
{
return CallbackReturn::ERROR;
}
// can only control one joint
if ( info_.joints.size( ) != 1 )
{
return CallbackReturn::ERROR;
}
// can only control in position
const auto & command_interfaces = info_.joints[0].command_interfaces;
if ( command_interfaces.size( ) != 1 )
{
return CallbackReturn::ERROR;
}
if ( command_interfaces[0].name != hardware_interface::HW_IF_POSITION )
{
return CallbackReturn::ERROR;
}
// can only give feedback state for position and velocity
const auto & state_interfaces = info_.joints[0].state_interfaces;
if ( state_interfaces.size( ) < 1 )
{
return CallbackReturn::ERROR;
}
for ( const auto & state_interface : state_interfaces )
{
if (
( state_interface.name != hardware_interface::HW_IF_POSITION ) &&
( state_interface.name != hardware_interface::HW_IF_VELOCITY ) )
{
return CallbackReturn::ERROR;
}
}
fprintf( stderr, "TestSingleJointActuator configured successfully.\n" );
return CallbackReturn::SUCCESS;
}
std::vector<StateInterface> export_state_interfaces( ) override
{
std::vector<StateInterface> state_interfaces;
const auto & joint_name = info_.joints[0].name;
state_interfaces.emplace_back( hardware_interface::StateInterface(
joint_name, hardware_interface::HW_IF_POSITION, &position_state_ ) );
state_interfaces.emplace_back( hardware_interface::StateInterface(
joint_name, hardware_interface::HW_IF_VELOCITY, &velocity_state_ ) );
return state_interfaces;
}
std::vector<CommandInterface> export_command_interfaces( ) override
{
std::vector<CommandInterface> command_interfaces;
const auto & joint_name = info_.joints[0].name;
command_interfaces.emplace_back( hardware_interface::CommandInterface(
joint_name, hardware_interface::HW_IF_POSITION, &position_command_ ) );
return command_interfaces;
}
return_type read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) override
{
return return_type::OK;
}
return_type write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) override
{
velocity_state_ = position_command_ - position_state_;
position_state_ = position_command_;
return return_type::OK;
}
private:
double position_state_ = 0.0;
double velocity_state_ = 0.0;
double position_command_ = 0.0;
};
} // namespace test_hardware_components
#include "pluginlib/class_list_macros.hpp" // NOLINT
PLUGINLIB_EXPORT_CLASS(
test_hardware_components::TestSingleJointActuator, hardware_interface::ActuatorInterface )
// Copyright 2021 Department of Engineering Cybernetics, NTNU
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <array>
#include <string>
#include <vector>
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
namespace test_hardware_components
{
24 class TestSystemCommandModes : public hardware_interface::SystemInterface
{
public:
CallbackReturn on_init( const hardware_interface::HardwareInfo & system_info ) override
{
if ( hardware_interface::SystemInterface::on_init( system_info ) != CallbackReturn::SUCCESS )
{
return CallbackReturn::ERROR;
}
// Can only control two joints
if ( info_.joints.size( ) != 2 )
{
return CallbackReturn::ERROR;
}
for ( const hardware_interface::ComponentInfo & joint : info_.joints )
{
// Can control in position or velocity
const auto & command_interfaces = joint.command_interfaces;
if ( command_interfaces.size( ) != 2 )
{
return CallbackReturn::ERROR;
}
for ( const auto & command_interface : command_interfaces )
{
if (
command_interface.name != hardware_interface::HW_IF_POSITION &&
command_interface.name != hardware_interface::HW_IF_VELOCITY )
{
return CallbackReturn::ERROR;
}
}
// Can give feedback state for position, velocity, and acceleration
const auto & state_interfaces = joint.state_interfaces;
if ( state_interfaces.size( ) != 2 )
{
return CallbackReturn::ERROR;
}
for ( const auto & state_interface : state_interfaces )
{
if (
state_interface.name != hardware_interface::HW_IF_POSITION &&
state_interface.name != hardware_interface::HW_IF_VELOCITY )
{
return CallbackReturn::ERROR;
}
}
}
fprintf( stderr, "TestSystemCommandModes configured successfully.\n" );
return CallbackReturn::SUCCESS;
}
std::vector<hardware_interface::StateInterface> export_state_interfaces( ) override
{
std::vector<hardware_interface::StateInterface> state_interfaces;
for ( auto i = 0u; i < info_.joints.size( ); i++ )
{
state_interfaces.emplace_back( hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i] ) );
state_interfaces.emplace_back( hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_state_[i] ) );
state_interfaces.emplace_back( hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_ACCELERATION, &acceleration_state_[i] ) );
}
return state_interfaces;
}
std::vector<hardware_interface::CommandInterface> export_command_interfaces( ) override
{
std::vector<hardware_interface::CommandInterface> command_interfaces;
for ( auto i = 0u; i < info_.joints.size( ); i++ )
{
command_interfaces.emplace_back( hardware_interface::CommandInterface(
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_command_[i] ) );
command_interfaces.emplace_back( hardware_interface::CommandInterface(
info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &velocity_command_[i] ) );
}
return command_interfaces;
}
hardware_interface::return_type read(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) override
{
return hardware_interface::return_type::OK;
}
hardware_interface::return_type write(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) override
{
return hardware_interface::return_type::OK;
}
hardware_interface::return_type prepare_command_mode_switch(
const std::vector<std::string> & start_interfaces,
const std::vector<std::string> & stop_interfaces ) override
{
// Starting interfaces
start_modes_.clear( );
stop_modes_.clear( );
for ( const auto & key : start_interfaces )
{
for ( auto i = 0u; i < info_.joints.size( ); i++ )
{
if ( key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION )
{
start_modes_.push_back( hardware_interface::HW_IF_POSITION );
}
if ( key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY )
{
start_modes_.push_back( hardware_interface::HW_IF_VELOCITY );
}
}
}
// Example Criteria 1 - Starting: All interfaces must be given a new mode at the same time
if ( start_modes_.size( ) != 0 && start_modes_.size( ) != info_.joints.size( ) )
{
return hardware_interface::return_type::ERROR;
}
// Stopping interfaces
for ( const auto & key : stop_interfaces )
{
for ( auto i = 0u; i < info_.joints.size( ); i++ )
{
if ( key.find( info_.joints[i].name ) != std::string::npos )
{
stop_modes_.push_back( true );
}
}
}
// Example Criteria 2 - Stopping: All joints must have the same command mode
if ( stop_modes_.size( ) != 0 && stop_modes_.size( ) != 2 && stop_modes_[0] != stop_modes_[1] )
{
return hardware_interface::return_type::ERROR;
}
return hardware_interface::return_type::OK;
}
hardware_interface::return_type perform_command_mode_switch(
const std::vector<std::string> & start_interfaces,
const std::vector<std::string> & /*stop_interfaces*/ ) override
{
// Test of failure in perform command mode switch
// Fail if given an empty list.
// This should never occur in a real system as the same start_interfaces list is sent to both
// prepare and perform, and an error should be handled in prepare.
if ( start_interfaces.size( ) == 0 )
{
return hardware_interface::return_type::ERROR;
}
return hardware_interface::return_type::OK;
}
private:
std::vector<std::string> start_modes_ = {"position", "position"};
std::vector<bool> stop_modes_ = {false, false};
std::array<double, 2> position_command_ = {0.0, 0.0};
std::array<double, 2> velocity_command_ = {0.0, 0.0};
std::array<double, 2> position_state_ = {0.0, 0.0};
std::array<double, 2> velocity_state_ = {0.0, 0.0};
std::array<double, 2> acceleration_state_ = {0.0, 0.0};
};
} // namespace test_hardware_components
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(
test_hardware_components::TestSystemCommandModes, hardware_interface::SystemInterface )
// Copyright 2020 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <array>
#include <memory>
#include <vector>
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
using hardware_interface::CommandInterface;
using hardware_interface::return_type;
using hardware_interface::StateInterface;
using hardware_interface::SystemInterface;
namespace test_hardware_components
{
29 class TestTwoJointSystem : public SystemInterface
{
CallbackReturn on_init( const hardware_interface::HardwareInfo & system_info ) override
{
if ( SystemInterface::on_init( system_info ) != CallbackReturn::SUCCESS )
{
return CallbackReturn::ERROR;
}
// can only control two joint
if ( info_.joints.size( ) != 2 )
{
return CallbackReturn::ERROR;
}
for ( const auto & joint : info_.joints )
{
// can only control in position
const auto & command_interfaces = joint.command_interfaces;
if ( command_interfaces.size( ) != 1 )
{
return CallbackReturn::ERROR;
}
if ( command_interfaces[0].name != hardware_interface::HW_IF_POSITION )
{
return CallbackReturn::ERROR;
}
// can only give feedback state for position and velocity
const auto & state_interfaces = joint.state_interfaces;
if ( state_interfaces.size( ) != 1 )
{
return CallbackReturn::ERROR;
}
if ( state_interfaces[0].name != hardware_interface::HW_IF_POSITION )
{
return CallbackReturn::ERROR;
}
}
fprintf( stderr, "TestTwoJointSystem configured successfully.\n" );
return CallbackReturn::SUCCESS;
}
std::vector<StateInterface> export_state_interfaces( ) override
{
std::vector<StateInterface> state_interfaces;
for ( auto i = 0u; i < info_.joints.size( ); ++i )
{
state_interfaces.emplace_back( hardware_interface::StateInterface(
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_state_[i] ) );
}
return state_interfaces;
}
std::vector<CommandInterface> export_command_interfaces( ) override
{
std::vector<CommandInterface> command_interfaces;
for ( auto i = 0u; i < info_.joints.size( ); ++i )
{
command_interfaces.emplace_back( hardware_interface::CommandInterface(
info_.joints[i].name, hardware_interface::HW_IF_POSITION, &position_command_[i] ) );
}
return command_interfaces;
}
return_type read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) override
{
return return_type::OK;
}
return_type write( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) override
{
return return_type::OK;
}
private:
std::array<double, 2> position_command_ = {0.0, 0.0};
std::array<double, 2> position_state_ = {0.0, 0.0};
};
} // namespace test_hardware_components
#include "pluginlib/class_list_macros.hpp" // NOLINT
PLUGINLIB_EXPORT_CLASS(
test_hardware_components::TestTwoJointSystem, hardware_interface::SystemInterface )
1 // Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <vector>
#include "hardware_interface/macros.hpp"
#include "gmock/gmock.h"
21 class TestMacros : public ::testing::Test
{
protected:
24 static void SetUpTestCase( ) {}
};
27 class A
{
};
31 TEST_F( TestMacros, throw_on_null )
{
int * i_ptr = nullptr;
// cppcheck-suppress unknownMacro
EXPECT_ANY_THROW( THROW_ON_NULLPTR( i_ptr ) );
A * a_ptr = nullptr;
EXPECT_ANY_THROW( THROW_ON_NULLPTR( a_ptr ) );
std::vector<int *> vec_ptr( 7 );
for ( auto ptr : vec_ptr )
{
EXPECT_ANY_THROW( THROW_ON_NULLPTR( ptr ) );
}
int ** i_ptr_ptr = &i_ptr;
EXPECT_ANY_THROW( THROW_ON_NULLPTR( *i_ptr_ptr ) );
/*
// undefined behavior
int * i_ptr;
THROW_ON_NULLPTR( i_ptr );
*/
}
56 TEST_F( TestMacros, throw_on_not_null )
{
int * i_ptr = new int( );
EXPECT_ANY_THROW( THROW_ON_NOT_NULLPTR( i_ptr ) );
A * a_ptr = new A( );
EXPECT_ANY_THROW( THROW_ON_NOT_NULLPTR( a_ptr ) );
std::vector<int *> vec_ptr;
for ( auto i : {0, 1, 2} )
{
vec_ptr.push_back( i_ptr );
EXPECT_ANY_THROW( THROW_ON_NOT_NULLPTR( vec_ptr[i] ) );
}
int ** i_ptr_ptr = &i_ptr;
EXPECT_ANY_THROW( THROW_ON_NOT_NULLPTR( *i_ptr_ptr ) );
delete a_ptr;
}
// Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// Authors: Karsten Knese, Denis Stogl
#include <gmock/gmock.h>
#include <algorithm>
#include <memory>
#include <string>
#include <unordered_map>
#include <vector>
#include "hardware_interface/actuator_interface.hpp"
#include "hardware_interface/resource_manager.hpp"
#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "ros2_control_test_assets/descriptions.hpp"
using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_CLASS_TYPE;
using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES;
using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME;
using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES;
using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_TYPE;
using ros2_control_test_assets::TEST_SENSOR_HARDWARE_CLASS_TYPE;
using ros2_control_test_assets::TEST_SENSOR_HARDWARE_COMMAND_INTERFACES;
using ros2_control_test_assets::TEST_SENSOR_HARDWARE_NAME;
using ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES;
using ros2_control_test_assets::TEST_SENSOR_HARDWARE_TYPE;
using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_CLASS_TYPE;
using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES;
using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_NAME;
using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_STATE_INTERFACES;
using ros2_control_test_assets::TEST_SYSTEM_HARDWARE_TYPE;
48 class TestResourceManager : public ::testing::Test
{
public:
51 static void SetUpTestCase( ) {}
53 void SetUp( ) {}
};
56 void set_components_state(
hardware_interface::ResourceManager & rm, const std::vector<std::string> & components,
const uint8_t state_id, const std::string & state_name )
{
auto int_components = components;
if ( int_components.empty( ) )
{
int_components = {"TestActuatorHardware", "TestSensorHardware", "TestSystemHardware"};
}
for ( const auto & component : int_components )
{
rclcpp_lifecycle::State state( state_id, state_name );
rm.set_component_state( component, state );
}
}
auto configure_components =
[]( hardware_interface::ResourceManager & rm, const std::vector<std::string> & components = {} )
{
set_components_state(
rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
hardware_interface::lifecycle_state_names::INACTIVE );
};
auto activate_components =
[]( hardware_interface::ResourceManager & rm, const std::vector<std::string> & components = {} )
{
set_components_state(
rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
hardware_interface::lifecycle_state_names::ACTIVE );
};
auto deactivate_components =
[]( hardware_interface::ResourceManager & rm, const std::vector<std::string> & components = {} )
{
set_components_state(
rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
hardware_interface::lifecycle_state_names::INACTIVE );
};
auto cleanup_components =
[]( hardware_interface::ResourceManager & rm, const std::vector<std::string> & components = {} )
{
set_components_state(
rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
hardware_interface::lifecycle_state_names::UNCONFIGURED );
};
auto shutdown_components =
[]( hardware_interface::ResourceManager & rm, const std::vector<std::string> & components = {} )
{
set_components_state(
rm, components, lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
hardware_interface::lifecycle_state_names::FINALIZED );
};
TEST_F( TestResourceManager, initialization_empty )
{
ASSERT_ANY_THROW( hardware_interface::ResourceManager rm( "" ) );
}
TEST_F( TestResourceManager, initialization_with_urdf )
{
ASSERT_NO_THROW(
hardware_interface::ResourceManager rm( ros2_control_test_assets::minimal_robot_urdf ) );
}
TEST_F( TestResourceManager, post_initialization_with_urdf )
{
hardware_interface::ResourceManager rm;
ASSERT_NO_THROW( rm.load_urdf( ros2_control_test_assets::minimal_robot_urdf ) );
}
TEST_F( TestResourceManager, initialization_with_urdf_manual_validation )
{
// we validate the results manually
hardware_interface::ResourceManager rm( ros2_control_test_assets::minimal_robot_urdf, false );
EXPECT_EQ( 1u, rm.actuator_components_size( ) );
EXPECT_EQ( 1u, rm.sensor_components_size( ) );
EXPECT_EQ( 1u, rm.system_components_size( ) );
auto state_interface_keys = rm.state_interface_keys( );
ASSERT_EQ( 11u, state_interface_keys.size( ) );
EXPECT_TRUE( rm.state_interface_exists( "joint1/position" ) );
EXPECT_TRUE( rm.state_interface_exists( "joint1/velocity" ) );
EXPECT_TRUE( rm.state_interface_exists( "sensor1/velocity" ) );
EXPECT_TRUE( rm.state_interface_exists( "joint2/position" ) );
EXPECT_TRUE( rm.state_interface_exists( "joint3/position" ) );
auto command_interface_keys = rm.command_interface_keys( );
ASSERT_EQ( 6u, command_interface_keys.size( ) );
EXPECT_TRUE( rm.command_interface_exists( "joint1/position" ) );
EXPECT_TRUE( rm.command_interface_exists( "joint2/velocity" ) );
EXPECT_TRUE( rm.command_interface_exists( "joint3/velocity" ) );
}
TEST_F( TestResourceManager, initialization_with_wrong_urdf )
{
// missing state keys
{
EXPECT_THROW(
hardware_interface::ResourceManager rm(
ros2_control_test_assets::minimal_robot_missing_state_keys_urdf ),
std::exception );
}
// missing command keys
{
EXPECT_THROW(
hardware_interface::ResourceManager rm(
ros2_control_test_assets::minimal_robot_missing_command_keys_urdf ),
std::exception );
}
}
TEST_F( TestResourceManager, initialization_with_urdf_unclaimed )
{
// we validate the results manually
hardware_interface::ResourceManager rm( ros2_control_test_assets::minimal_robot_urdf );
auto command_interface_keys = rm.command_interface_keys( );
for ( const auto & key : command_interface_keys )
{
EXPECT_FALSE( rm.command_interface_is_claimed( key ) );
}
// state interfaces don't have to be locked, hence any arbitrary key
// should return false.
auto state_interface_keys = rm.state_interface_keys( );
for ( const auto & key : state_interface_keys )
{
EXPECT_FALSE( rm.command_interface_is_claimed( key ) );
}
}
TEST_F( TestResourceManager, resource_claiming )
{
hardware_interface::ResourceManager rm( ros2_control_test_assets::minimal_robot_urdf );
// Activate components to get all interfaces available
activate_components( rm );
const auto key = "joint1/position";
EXPECT_TRUE( rm.command_interface_is_available( key ) );
EXPECT_FALSE( rm.command_interface_is_claimed( key ) );
{
auto position_command_interface = rm.claim_command_interface( key );
EXPECT_TRUE( rm.command_interface_is_available( key ) );
EXPECT_TRUE( rm.command_interface_is_claimed( key ) );
{
EXPECT_ANY_THROW( rm.claim_command_interface( key ) );
EXPECT_TRUE( rm.command_interface_is_available( key ) );
}
}
EXPECT_TRUE( rm.command_interface_is_available( key ) );
EXPECT_FALSE( rm.command_interface_is_claimed( key ) );
// command interfaces can only be claimed once
for ( const auto & key :
{"joint1/position", "joint1/position", "joint1/position", "joint2/velocity",
"joint3/velocity"} )
{
{
auto interface = rm.claim_command_interface( key );
EXPECT_TRUE( rm.command_interface_is_available( key ) );
EXPECT_TRUE( rm.command_interface_is_claimed( key ) );
{
EXPECT_ANY_THROW( rm.claim_command_interface( key ) );
EXPECT_TRUE( rm.command_interface_is_available( key ) );
}
}
EXPECT_TRUE( rm.command_interface_is_available( key ) );
EXPECT_FALSE( rm.command_interface_is_claimed( key ) );
}
// TODO( destogl ): This claim test is not true.... can not be...
// state interfaces can be claimed multiple times
for ( const auto & key :
{"joint1/position", "joint1/velocity", "sensor1/velocity", "joint2/position",
"joint3/position"} )
{
{
EXPECT_TRUE( rm.state_interface_is_available( key ) );
auto interface = rm.claim_state_interface( key );
{
EXPECT_TRUE( rm.state_interface_is_available( key ) );
EXPECT_NO_THROW( rm.claim_state_interface( key ) );
}
}
}
std::vector<hardware_interface::LoanedCommandInterface> interfaces;
const auto interface_names = {"joint1/position", "joint2/velocity", "joint3/velocity"};
for ( const auto & key : interface_names )
{
EXPECT_TRUE( rm.command_interface_is_available( key ) );
interfaces.emplace_back( rm.claim_command_interface( key ) );
}
for ( const auto & key : interface_names )
{
EXPECT_TRUE( rm.command_interface_is_available( key ) );
EXPECT_TRUE( rm.command_interface_is_claimed( key ) );
}
interfaces.clear( );
for ( const auto & key : interface_names )
{
EXPECT_TRUE( rm.command_interface_is_available( key ) );
EXPECT_FALSE( rm.command_interface_is_claimed( key ) );
}
}
class ExternalComponent : public hardware_interface::ActuatorInterface
{
std::vector<hardware_interface::StateInterface> export_state_interfaces( ) override
{
std::vector<hardware_interface::StateInterface> state_interfaces;
state_interfaces.emplace_back(
hardware_interface::StateInterface( "external_joint", "external_state_interface", nullptr ) );
return state_interfaces;
}
std::vector<hardware_interface::CommandInterface> export_command_interfaces( ) override
{
std::vector<hardware_interface::CommandInterface> command_interfaces;
command_interfaces.emplace_back( hardware_interface::CommandInterface(
"external_joint", "external_command_interface", nullptr ) );
return command_interfaces;
}
std::string get_name( ) const override { return "ExternalComponent"; }
hardware_interface::return_type read(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) override
{
return hardware_interface::return_type::OK;
}
hardware_interface::return_type write(
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ ) override
{
return hardware_interface::return_type::OK;
}
};
TEST_F( TestResourceManager, post_initialization_add_components )
{
// we validate the results manually
hardware_interface::ResourceManager rm( ros2_control_test_assets::minimal_robot_urdf, false );
// Activate components to get all interfaces available
activate_components( rm );
EXPECT_EQ( 1u, rm.actuator_components_size( ) );
EXPECT_EQ( 1u, rm.sensor_components_size( ) );
EXPECT_EQ( 1u, rm.system_components_size( ) );
ASSERT_EQ( 11u, rm.state_interface_keys( ).size( ) );
ASSERT_EQ( 6u, rm.command_interface_keys( ).size( ) );
hardware_interface::HardwareInfo external_component_hw_info;
external_component_hw_info.name = "ExternalComponent";
external_component_hw_info.type = "actuator";
rm.import_component( std::make_unique<ExternalComponent>( ), external_component_hw_info );
EXPECT_EQ( 2u, rm.actuator_components_size( ) );
ASSERT_EQ( 12u, rm.state_interface_keys( ).size( ) );
EXPECT_TRUE( rm.state_interface_exists( "external_joint/external_state_interface" ) );
ASSERT_EQ( 7u, rm.command_interface_keys( ).size( ) );
EXPECT_TRUE( rm.command_interface_exists( "external_joint/external_command_interface" ) );
auto status_map = rm.get_components_status( );
EXPECT_EQ(
status_map["ExternalComponent"].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
configure_components( rm, {"ExternalComponent"} );
status_map = rm.get_components_status( );
EXPECT_EQ(
status_map["ExternalComponent"].state.id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
activate_components( rm, {"ExternalComponent"} );
status_map = rm.get_components_status( );
EXPECT_EQ(
status_map["ExternalComponent"].state.id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
EXPECT_NO_THROW( rm.claim_state_interface( "external_joint/external_state_interface" ) );
EXPECT_NO_THROW( rm.claim_command_interface( "external_joint/external_command_interface" ) );
}
TEST_F( TestResourceManager, default_prepare_perform_switch )
{
hardware_interface::ResourceManager rm( ros2_control_test_assets::minimal_robot_urdf );
// Activate components to get all interfaces available
activate_components( rm );
EXPECT_TRUE( rm.prepare_command_mode_switch( {""}, {""} ) );
EXPECT_TRUE( rm.perform_command_mode_switch( {""}, {""} ) );
}
const auto hardware_resources_command_modes =
R"(
<ros2_control name="TestSystemCommandModes" type="system">
<hardware>
<plugin>test_hardware_components/TestSystemCommandModes</plugin>
</hardware>
<joint name="joint1">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="joint2">
<command_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
</ros2_control>
)";
const auto command_mode_urdf = std::string( ros2_control_test_assets::urdf_head ) +
std::string( hardware_resources_command_modes ) +
std::string( ros2_control_test_assets::urdf_tail );
TEST_F( TestResourceManager, custom_prepare_perform_switch )
{
hardware_interface::ResourceManager rm( command_mode_urdf );
// Scenarios defined by example criteria
std::vector<std::string> empty_keys = {};
std::vector<std::string> irrelevant_keys = {"elbow_joint/position", "should_joint/position"};
std::vector<std::string> illegal_single_key = {"joint1/position"};
std::vector<std::string> legal_keys_position = {"joint1/position", "joint2/position"};
std::vector<std::string> legal_keys_velocity = {"joint1/velocity", "joint2/velocity"};
// Default behavior for empty key lists
EXPECT_TRUE( rm.prepare_command_mode_switch( empty_keys, empty_keys ) );
// Default behavior when given irrelevant keys
EXPECT_TRUE( rm.prepare_command_mode_switch( irrelevant_keys, irrelevant_keys ) );
EXPECT_TRUE( rm.prepare_command_mode_switch( irrelevant_keys, empty_keys ) );
EXPECT_TRUE( rm.prepare_command_mode_switch( empty_keys, irrelevant_keys ) );
// The test hardware interface has a criteria that both joints must change mode
EXPECT_FALSE( rm.prepare_command_mode_switch( illegal_single_key, illegal_single_key ) );
EXPECT_FALSE( rm.prepare_command_mode_switch( illegal_single_key, empty_keys ) );
EXPECT_FALSE( rm.prepare_command_mode_switch( empty_keys, illegal_single_key ) );
// Test legal start keys
EXPECT_TRUE( rm.prepare_command_mode_switch( legal_keys_position, legal_keys_position ) );
EXPECT_TRUE( rm.prepare_command_mode_switch( legal_keys_velocity, legal_keys_velocity ) );
EXPECT_TRUE( rm.prepare_command_mode_switch( legal_keys_position, empty_keys ) );
EXPECT_TRUE( rm.prepare_command_mode_switch( empty_keys, legal_keys_position ) );
EXPECT_TRUE( rm.prepare_command_mode_switch( legal_keys_velocity, empty_keys ) );
EXPECT_TRUE( rm.prepare_command_mode_switch( empty_keys, legal_keys_velocity ) );
// Test rejection from perform_command_mode_switch, test hardware rejects empty start sets
EXPECT_TRUE( rm.perform_command_mode_switch( legal_keys_position, legal_keys_position ) );
EXPECT_FALSE( rm.perform_command_mode_switch( empty_keys, empty_keys ) );
EXPECT_FALSE( rm.perform_command_mode_switch( empty_keys, legal_keys_position ) );
}
TEST_F( TestResourceManager, resource_status )
{
hardware_interface::ResourceManager rm( ros2_control_test_assets::minimal_robot_urdf );
auto status_map = rm.get_components_status( );
// name
EXPECT_EQ( status_map[TEST_ACTUATOR_HARDWARE_NAME].name, TEST_ACTUATOR_HARDWARE_NAME );
EXPECT_EQ( status_map[TEST_SENSOR_HARDWARE_NAME].name, TEST_SENSOR_HARDWARE_NAME );
EXPECT_EQ( status_map[TEST_SYSTEM_HARDWARE_NAME].name, TEST_SYSTEM_HARDWARE_NAME );
// type
EXPECT_EQ( status_map[TEST_ACTUATOR_HARDWARE_NAME].type, TEST_ACTUATOR_HARDWARE_TYPE );
EXPECT_EQ( status_map[TEST_SENSOR_HARDWARE_NAME].type, TEST_SENSOR_HARDWARE_TYPE );
EXPECT_EQ( status_map[TEST_SYSTEM_HARDWARE_NAME].type, TEST_SYSTEM_HARDWARE_TYPE );
// class_type
EXPECT_EQ( status_map[TEST_ACTUATOR_HARDWARE_NAME].class_type, TEST_ACTUATOR_HARDWARE_CLASS_TYPE );
EXPECT_EQ( status_map[TEST_SENSOR_HARDWARE_NAME].class_type, TEST_SENSOR_HARDWARE_CLASS_TYPE );
EXPECT_EQ( status_map[TEST_SYSTEM_HARDWARE_NAME].class_type, TEST_SYSTEM_HARDWARE_CLASS_TYPE );
// state
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::UNCONFIGURED );
auto check_interfaces = [](
const std::vector<std::string> & registered_interfaces,
const std::vector<std::string> & interface_names )
{
for ( const std::string & interface : interface_names )
{
auto it = std::find( registered_interfaces.begin( ), registered_interfaces.end( ), interface );
EXPECT_NE( it, registered_interfaces.end( ) );
}
};
check_interfaces(
status_map[TEST_ACTUATOR_HARDWARE_NAME].command_interfaces,
TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES );
EXPECT_TRUE( status_map[TEST_SENSOR_HARDWARE_NAME].command_interfaces.empty( ) );
check_interfaces(
status_map[TEST_SYSTEM_HARDWARE_NAME].command_interfaces,
TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES );
check_interfaces(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state_interfaces,
TEST_ACTUATOR_HARDWARE_STATE_INTERFACES );
EXPECT_NE(
std::find(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state_interfaces.begin( ),
status_map[TEST_ACTUATOR_HARDWARE_NAME].state_interfaces.end( ),
"joint1/some_unlisted_interface" ),
status_map[TEST_ACTUATOR_HARDWARE_NAME].state_interfaces.end( ) );
check_interfaces(
status_map[TEST_SENSOR_HARDWARE_NAME].state_interfaces, TEST_SENSOR_HARDWARE_STATE_INTERFACES );
check_interfaces(
status_map[TEST_SYSTEM_HARDWARE_NAME].state_interfaces, TEST_SYSTEM_HARDWARE_STATE_INTERFACES );
}
TEST_F( TestResourceManager, lifecycle_all_resources )
{
hardware_interface::ResourceManager rm( ros2_control_test_assets::minimal_robot_urdf );
// All resources start as UNCONFIGURED
{
auto status_map = rm.get_components_status( );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::UNCONFIGURED );
}
configure_components( rm );
{
auto status_map = rm.get_components_status( );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::INACTIVE );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::INACTIVE );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::INACTIVE );
}
activate_components( rm );
{
auto status_map = rm.get_components_status( );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::ACTIVE );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::ACTIVE );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::ACTIVE );
}
deactivate_components( rm );
{
auto status_map = rm.get_components_status( );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::INACTIVE );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::INACTIVE );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::INACTIVE );
}
cleanup_components( rm );
{
auto status_map = rm.get_components_status( );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::UNCONFIGURED );
}
shutdown_components( rm );
{
auto status_map = rm.get_components_status( );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::FINALIZED );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::FINALIZED );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::FINALIZED );
}
}
TEST_F( TestResourceManager, lifecycle_individual_resources )
{
hardware_interface::ResourceManager rm( ros2_control_test_assets::minimal_robot_urdf );
// All resources start as UNCONFIGURED
{
auto status_map = rm.get_components_status( );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::UNCONFIGURED );
}
configure_components( rm, {TEST_ACTUATOR_HARDWARE_NAME} );
{
auto status_map = rm.get_components_status( );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::INACTIVE );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::UNCONFIGURED );
}
activate_components( rm, {TEST_ACTUATOR_HARDWARE_NAME} );
{
auto status_map = rm.get_components_status( );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::ACTIVE );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::UNCONFIGURED );
}
configure_components( rm, {TEST_SENSOR_HARDWARE_NAME, TEST_SYSTEM_HARDWARE_NAME} );
{
auto status_map = rm.get_components_status( );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::ACTIVE );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::INACTIVE );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::INACTIVE );
}
activate_components( rm, {TEST_SENSOR_HARDWARE_NAME, TEST_SYSTEM_HARDWARE_NAME} );
{
auto status_map = rm.get_components_status( );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::ACTIVE );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::ACTIVE );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::ACTIVE );
}
deactivate_components( rm, {TEST_ACTUATOR_HARDWARE_NAME, TEST_SENSOR_HARDWARE_NAME} );
{
auto status_map = rm.get_components_status( );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::INACTIVE );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::INACTIVE );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::ACTIVE );
}
cleanup_components( rm, {TEST_SENSOR_HARDWARE_NAME} );
{
auto status_map = rm.get_components_status( );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::INACTIVE );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::ACTIVE );
}
shutdown_components( rm, {TEST_ACTUATOR_HARDWARE_NAME, TEST_SYSTEM_HARDWARE_NAME} );
{
auto status_map = rm.get_components_status( );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::FINALIZED );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::UNCONFIGURED );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::FINALIZED );
}
// TODO( destogl ): Watch-out this fails in output, why is this not caught?!!!
shutdown_components( rm, {TEST_SENSOR_HARDWARE_NAME} );
{
auto status_map = rm.get_components_status( );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED );
EXPECT_EQ(
status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::FINALIZED );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED );
EXPECT_EQ(
status_map[TEST_SENSOR_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::FINALIZED );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.id( ),
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED );
EXPECT_EQ(
status_map[TEST_SYSTEM_HARDWARE_NAME].state.label( ),
hardware_interface::lifecycle_state_names::FINALIZED );
}
}
TEST_F( TestResourceManager, resource_availability_and_claiming_in_lifecycle )
{
using std::placeholders::_1;
hardware_interface::ResourceManager rm( ros2_control_test_assets::minimal_robot_urdf );
auto check_interfaces =
[]( const std::vector<std::string> & interface_names, auto check_method, bool expected_result )
{
for ( const auto & interface : interface_names )
{
EXPECT_EQ( check_method( interface ), expected_result );
}
};
auto check_interface_claiming = [&](
const std::vector<std::string> & state_interface_names,
const std::vector<std::string> & command_interface_names,
bool expected_result )
{
std::vector<hardware_interface::LoanedStateInterface> states;
std::vector<hardware_interface::LoanedCommandInterface> commands;
if ( expected_result )
{
for ( const auto & key : state_interface_names )
{
EXPECT_NO_THROW( states.emplace_back( rm.claim_state_interface( key ) ) );
}
for ( const auto & key : command_interface_names )
{
EXPECT_NO_THROW( commands.emplace_back( rm.claim_command_interface( key ) ) );
}
}
else
{
for ( const auto & key : state_interface_names )
{
EXPECT_ANY_THROW( states.emplace_back( rm.claim_state_interface( key ) ) );
}
for ( const auto & key : command_interface_names )
{
EXPECT_ANY_THROW( commands.emplace_back( rm.claim_command_interface( key ) ) );
}
}
check_interfaces(
command_interface_names,
std::bind( &hardware_interface::ResourceManager::command_interface_is_claimed, &rm, _1 ),
expected_result );
};
// All resources start as UNCONFIGURED - All interfaces are imported but not available
{
check_interfaces(
TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES,
std::bind( &hardware_interface::ResourceManager::command_interface_exists, &rm, _1 ), true );
check_interfaces(
TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,
std::bind( &hardware_interface::ResourceManager::command_interface_exists, &rm, _1 ), true );
check_interfaces(
TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_exists, &rm, _1 ), true );
check_interfaces(
TEST_SENSOR_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_exists, &rm, _1 ), true );
check_interfaces(
TEST_SYSTEM_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_exists, &rm, _1 ), true );
check_interfaces(
TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES,
std::bind( &hardware_interface::ResourceManager::command_interface_is_available, &rm, _1 ),
false );
check_interfaces(
TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,
std::bind( &hardware_interface::ResourceManager::command_interface_is_available, &rm, _1 ),
false );
check_interfaces(
TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_is_available, &rm, _1 ),
false );
check_interfaces(
TEST_SENSOR_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_is_available, &rm, _1 ),
false );
check_interfaces(
TEST_SYSTEM_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_is_available, &rm, _1 ),
false );
}
// Nothing can be claimed
{
check_interface_claiming(
TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, false );
check_interface_claiming( TEST_SENSOR_HARDWARE_STATE_INTERFACES, {}, false );
check_interface_claiming(
TEST_SYSTEM_HARDWARE_STATE_INTERFACES, TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, false );
}
// When actuator is configured all interfaces become available
configure_components( rm, {TEST_ACTUATOR_HARDWARE_NAME} );
{
check_interfaces(
{"joint1/position"},
std::bind( &hardware_interface::ResourceManager::command_interface_is_available, &rm, _1 ),
true );
check_interfaces(
{"joint1/max_velocity"},
std::bind( &hardware_interface::ResourceManager::command_interface_is_available, &rm, _1 ),
true );
check_interfaces(
TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,
std::bind( &hardware_interface::ResourceManager::command_interface_is_available, &rm, _1 ),
false );
check_interfaces(
TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_is_available, &rm, _1 ), true );
check_interfaces(
TEST_SENSOR_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_is_available, &rm, _1 ),
false );
check_interfaces(
TEST_SYSTEM_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_is_available, &rm, _1 ),
false );
}
// Can claim Actuator's interfaces
{
check_interface_claiming( {}, {"joint1/position"}, true );
check_interface_claiming(
TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, {"joint1/max_velocity"}, true );
check_interface_claiming( TEST_SENSOR_HARDWARE_STATE_INTERFACES, {}, false );
check_interface_claiming(
TEST_SYSTEM_HARDWARE_STATE_INTERFACES, TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, false );
}
// When actuator is activated all state- and command- interfaces become available
activate_components( rm, {TEST_ACTUATOR_HARDWARE_NAME} );
{
check_interfaces(
TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES,
std::bind( &hardware_interface::ResourceManager::command_interface_is_available, &rm, _1 ),
true );
check_interfaces(
TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,
std::bind( &hardware_interface::ResourceManager::command_interface_is_available, &rm, _1 ),
false );
check_interfaces(
TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_is_available, &rm, _1 ), true );
check_interfaces(
TEST_SENSOR_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_is_available, &rm, _1 ),
false );
check_interfaces(
TEST_SYSTEM_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_is_available, &rm, _1 ),
false );
}
// Can claim all Actuator's state interfaces and command interfaces
{
check_interface_claiming(
TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, true );
check_interface_claiming( TEST_SENSOR_HARDWARE_STATE_INTERFACES, {}, false );
check_interface_claiming(
TEST_SYSTEM_HARDWARE_STATE_INTERFACES, TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, false );
}
// Check if all interfaces still exits
{
check_interfaces(
TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES,
std::bind( &hardware_interface::ResourceManager::command_interface_exists, &rm, _1 ), true );
check_interfaces(
TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,
std::bind( &hardware_interface::ResourceManager::command_interface_exists, &rm, _1 ), true );
check_interfaces(
TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_exists, &rm, _1 ), true );
check_interfaces(
TEST_SENSOR_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_exists, &rm, _1 ), true );
check_interfaces(
TEST_SYSTEM_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_exists, &rm, _1 ), true );
}
// When Sensor and System are configured their state-
// and command- interfaces are available
configure_components( rm, {TEST_SENSOR_HARDWARE_NAME, TEST_SYSTEM_HARDWARE_NAME} );
{
check_interfaces(
TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES,
std::bind( &hardware_interface::ResourceManager::command_interface_is_available, &rm, _1 ),
true );
check_interfaces(
{"joint2/velocity", "joint3/velocity"},
std::bind( &hardware_interface::ResourceManager::command_interface_is_available, &rm, _1 ),
true );
check_interfaces(
{"joint2/max_acceleration", "configuration/max_tcp_jerk"},
std::bind( &hardware_interface::ResourceManager::command_interface_is_available, &rm, _1 ),
true );
check_interfaces(
TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_is_available, &rm, _1 ), true );
check_interfaces(
TEST_SENSOR_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_is_available, &rm, _1 ), true );
check_interfaces(
TEST_SYSTEM_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_is_available, &rm, _1 ), true );
}
// Can claim:
// - all Actuator's state interfaces and command interfaces
// - sensor's state interfaces
// - system's state and command interfaces
{
check_interface_claiming(
TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, true );
check_interface_claiming( TEST_SENSOR_HARDWARE_STATE_INTERFACES, {}, true );
check_interface_claiming( {}, {"joint2/velocity", "joint3/velocity"}, true );
check_interface_claiming(
TEST_SYSTEM_HARDWARE_STATE_INTERFACES,
{"joint2/max_acceleration", "configuration/max_tcp_jerk"}, true );
}
// All active - everything available
activate_components( rm, {TEST_SENSOR_HARDWARE_NAME, TEST_SYSTEM_HARDWARE_NAME} );
{
check_interfaces(
TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES,
std::bind( &hardware_interface::ResourceManager::command_interface_is_available, &rm, _1 ),
true );
check_interfaces(
TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,
std::bind( &hardware_interface::ResourceManager::command_interface_is_available, &rm, _1 ),
true );
check_interfaces(
TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_is_available, &rm, _1 ), true );
check_interfaces(
TEST_SENSOR_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_is_available, &rm, _1 ), true );
check_interfaces(
TEST_SYSTEM_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_is_available, &rm, _1 ), true );
}
// Can claim everything
// - actuator's state interfaces and command interfaces
// - sensor's state interfaces
// - system's state and non-moving command interfaces
{
check_interface_claiming(
TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, true );
check_interface_claiming( TEST_SENSOR_HARDWARE_STATE_INTERFACES, {}, true );
check_interface_claiming(
TEST_SYSTEM_HARDWARE_STATE_INTERFACES, TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, true );
}
// When deactivated - movement interfaces are not available anymore
deactivate_components( rm, {TEST_ACTUATOR_HARDWARE_NAME, TEST_SENSOR_HARDWARE_NAME} );
{
check_interfaces(
{"joint1/position"},
std::bind( &hardware_interface::ResourceManager::command_interface_is_available, &rm, _1 ),
true );
check_interfaces(
{"joint1/max_velocity"},
std::bind( &hardware_interface::ResourceManager::command_interface_is_available, &rm, _1 ),
true );
check_interfaces(
TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,
std::bind( &hardware_interface::ResourceManager::command_interface_is_available, &rm, _1 ),
true );
check_interfaces(
TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_is_available, &rm, _1 ), true );
check_interfaces(
TEST_SENSOR_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_is_available, &rm, _1 ), true );
check_interfaces(
TEST_SYSTEM_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_is_available, &rm, _1 ), true );
}
// Can claim everything
// - actuator's state and command interfaces
// - sensor's state interfaces
// - system's state and command interfaces
{
check_interface_claiming( {}, {"joint1/position"}, true );
check_interface_claiming(
TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, {"joint1/max_velocity"}, true );
check_interface_claiming( TEST_SENSOR_HARDWARE_STATE_INTERFACES, {}, true );
check_interface_claiming(
TEST_SYSTEM_HARDWARE_STATE_INTERFACES, TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, true );
}
// When sensor is cleaned up the interfaces are not available anymore
cleanup_components( rm, {TEST_SENSOR_HARDWARE_NAME} );
{
check_interfaces(
{"joint1/position"},
std::bind( &hardware_interface::ResourceManager::command_interface_is_available, &rm, _1 ),
true );
check_interfaces(
{"joint1/max_velocity"},
std::bind( &hardware_interface::ResourceManager::command_interface_is_available, &rm, _1 ),
true );
check_interfaces(
TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,
std::bind( &hardware_interface::ResourceManager::command_interface_is_available, &rm, _1 ),
true );
check_interfaces(
TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_is_available, &rm, _1 ), true );
check_interfaces(
TEST_SENSOR_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_is_available, &rm, _1 ),
false );
check_interfaces(
TEST_SYSTEM_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_is_available, &rm, _1 ), true );
}
// Can claim everything
// - actuator's state and command interfaces
// - no sensor's interface
// - system's state and command interfaces
{
check_interface_claiming( {}, {"joint1/position"}, true );
check_interface_claiming(
TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, {"joint1/max_velocity"}, true );
check_interface_claiming( TEST_SENSOR_HARDWARE_STATE_INTERFACES, {}, false );
check_interface_claiming(
TEST_SYSTEM_HARDWARE_STATE_INTERFACES, TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, true );
}
// Check if all interfaces still exits
{
check_interfaces(
TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES,
std::bind( &hardware_interface::ResourceManager::command_interface_exists, &rm, _1 ), true );
check_interfaces(
TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES,
std::bind( &hardware_interface::ResourceManager::command_interface_exists, &rm, _1 ), true );
check_interfaces(
TEST_ACTUATOR_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_exists, &rm, _1 ), true );
check_interfaces(
TEST_SENSOR_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_exists, &rm, _1 ), true );
check_interfaces(
TEST_SYSTEM_HARDWARE_STATE_INTERFACES,
std::bind( &hardware_interface::ResourceManager::state_interface_exists, &rm, _1 ), true );
}
}
TEST_F( TestResourceManager, managing_controllers_reference_interfaces )
{
hardware_interface::ResourceManager rm( ros2_control_test_assets::minimal_robot_urdf );
std::string CONTROLLER_NAME = "test_controller";
std::vector<std::string> REFERENCE_INTERFACE_NAMES = {"input1", "input2", "input3"};
std::vector<std::string> FULL_REFERENCE_INTERFACE_NAMES = {
CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[0],
CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[1],
CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[2]};
std::vector<hardware_interface::CommandInterface> reference_interfaces;
std::vector<double> reference_interface_values = {1.0, 2.0, 3.0};
for ( size_t i = 0; i < REFERENCE_INTERFACE_NAMES.size( ); ++i )
{
reference_interfaces.push_back( hardware_interface::CommandInterface(
CONTROLLER_NAME, REFERENCE_INTERFACE_NAMES[i], &( reference_interface_values[i] ) ) );
}
rm.import_controller_reference_interfaces( CONTROLLER_NAME, reference_interfaces );
ASSERT_THAT(
rm.get_controller_reference_interface_names( CONTROLLER_NAME ),
testing::ElementsAreArray( FULL_REFERENCE_INTERFACE_NAMES ) );
// check that all interfaces are imported properly
for ( const auto & interface : FULL_REFERENCE_INTERFACE_NAMES )
{
EXPECT_TRUE( rm.command_interface_exists( interface ) );
EXPECT_FALSE( rm.command_interface_is_available( interface ) );
EXPECT_FALSE( rm.command_interface_is_claimed( interface ) );
}
// make interface available
rm.make_controller_reference_interfaces_available( CONTROLLER_NAME );
for ( const auto & interface : FULL_REFERENCE_INTERFACE_NAMES )
{
EXPECT_TRUE( rm.command_interface_exists( interface ) );
EXPECT_TRUE( rm.command_interface_is_available( interface ) );
EXPECT_FALSE( rm.command_interface_is_claimed( interface ) );
}
// try to make interfaces available from unknown controller
EXPECT_THROW(
rm.make_controller_reference_interfaces_available( "unknown_controller" ), std::out_of_range );
// claim interfaces in a scope that deletes them after
{
auto claimed_itf1 = rm.claim_command_interface( FULL_REFERENCE_INTERFACE_NAMES[0] );
auto claimed_itf3 = rm.claim_command_interface( FULL_REFERENCE_INTERFACE_NAMES[2] );
for ( const auto & interface : FULL_REFERENCE_INTERFACE_NAMES )
{
EXPECT_TRUE( rm.command_interface_exists( interface ) );
EXPECT_TRUE( rm.command_interface_is_available( interface ) );
}
EXPECT_TRUE( rm.command_interface_is_claimed( FULL_REFERENCE_INTERFACE_NAMES[0] ) );
EXPECT_FALSE( rm.command_interface_is_claimed( FULL_REFERENCE_INTERFACE_NAMES[1] ) );
EXPECT_TRUE( rm.command_interface_is_claimed( FULL_REFERENCE_INTERFACE_NAMES[2] ) );
// access interface value
EXPECT_EQ( claimed_itf1.get_value( ), 1.0 );
EXPECT_EQ( claimed_itf3.get_value( ), 3.0 );
claimed_itf1.set_value( 11.1 );
claimed_itf3.set_value( 33.3 );
EXPECT_EQ( claimed_itf1.get_value( ), 11.1 );
EXPECT_EQ( claimed_itf3.get_value( ), 33.3 );
EXPECT_EQ( reference_interface_values[0], 11.1 );
EXPECT_EQ( reference_interface_values[1], 2.0 );
EXPECT_EQ( reference_interface_values[2], 33.3 );
}
// interfaces should be released now, but still managed by resource manager
for ( const auto & interface : FULL_REFERENCE_INTERFACE_NAMES )
{
EXPECT_TRUE( rm.command_interface_exists( interface ) );
EXPECT_TRUE( rm.command_interface_is_available( interface ) );
EXPECT_FALSE( rm.command_interface_is_claimed( interface ) );
}
// make interfaces unavailable
rm.make_controller_reference_interfaces_unavailable( CONTROLLER_NAME );
for ( const auto & interface : FULL_REFERENCE_INTERFACE_NAMES )
{
EXPECT_TRUE( rm.command_interface_exists( interface ) );
EXPECT_FALSE( rm.command_interface_is_available( interface ) );
EXPECT_FALSE( rm.command_interface_is_claimed( interface ) );
}
// try to make interfaces unavailable from unknown controller
EXPECT_THROW(
rm.make_controller_reference_interfaces_unavailable( "unknown_controller" ), std::out_of_range );
// Last written values should stay
EXPECT_EQ( reference_interface_values[0], 11.1 );
EXPECT_EQ( reference_interface_values[1], 2.0 );
EXPECT_EQ( reference_interface_values[2], 33.3 );
// remove reference interfaces from resource manager
rm.remove_controller_reference_interfaces( CONTROLLER_NAME );
// they should not exist in resource manager
for ( const auto & interface : FULL_REFERENCE_INTERFACE_NAMES )
{
EXPECT_FALSE( rm.command_interface_exists( interface ) );
EXPECT_FALSE( rm.command_interface_is_available( interface ) );
}
// try to remove interfaces from unknown controller
EXPECT_THROW(
rm.make_controller_reference_interfaces_unavailable( "unknown_controller" ), std::out_of_range );
}
// Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/// \author Adolfo Rodriguez Tsouroukdissian
#ifndef JOINT_LIMITS__JOINT_LIMITS_HPP_
#define JOINT_LIMITS__JOINT_LIMITS_HPP_
#include <limits>
#include <sstream>
#include <string>
namespace joint_limits
{
/**
* JointLimits structure stores values from from yaml definition or `<limits>` tag in URDF.
* The mapping from URDF attributes to members is the following:
* lower --> min_position
* upper --> max_position
* velocity --> max_velocity
* effort --> max_effort
*/
struct JointLimits
{
JointLimits( )
: min_position( std::numeric_limits<double>::quiet_NaN( ) ),
max_position( std::numeric_limits<double>::quiet_NaN( ) ),
max_velocity( std::numeric_limits<double>::quiet_NaN( ) ),
max_acceleration( std::numeric_limits<double>::quiet_NaN( ) ),
max_jerk( std::numeric_limits<double>::quiet_NaN( ) ),
max_effort( std::numeric_limits<double>::quiet_NaN( ) ),
has_position_limits( false ),
has_velocity_limits( false ),
has_acceleration_limits( false ),
has_jerk_limits( false ),
has_effort_limits( false ),
angle_wraparound( false )
{
}
double min_position;
double max_position;
double max_velocity;
double max_acceleration;
double max_jerk;
double max_effort;
bool has_position_limits;
bool has_velocity_limits;
bool has_acceleration_limits;
bool has_jerk_limits;
bool has_effort_limits;
bool angle_wraparound;
66 std::string to_string( )
{
std::stringstream ss_output;
ss_output << " has position limits: " << ( has_position_limits ? "true" : "false" ) << "["
<< min_position << ", " << max_position << "]\n";
ss_output << " has velocity limits: " << ( has_velocity_limits ? "true" : "false" ) << "["
<< max_velocity << "]\n";
ss_output << " has acceleration limits: " << ( has_acceleration_limits ? "true" : "false" )
<< " [" << max_acceleration << "]\n";
ss_output << " has jerk limits: " << ( has_jerk_limits ? "true" : "false" ) << "[" << max_jerk
<< "]\n";
ss_output << " has effort limits: " << ( has_effort_limits ? "true" : "false" ) << "["
<< max_effort << "]\n";
ss_output << " angle wraparound: " << ( angle_wraparound ? "true" : "false" );
return ss_output.str( );
}
};
/**
* SoftJointLimits stores values from the `<safety_controller>` tag of URDF.
* The meaning of the fields are:
*
* An element can contain the following attributes:
*
* **soft_lower_limit** ( optional, defaults to 0 ) - An attribute specifying the lower joint boundary
* where the safety controller starts limiting the position of the joint. This limit needs to be
* larger than the lower joint limit ( see above ). See See safety limits for more details.
*
* **soft_upper_limit** ( optional, defaults to 0 ) - An attribute specifying the upper joint boundary
* where the safety controller starts limiting the position of the joint. This limit needs to be
* smaller than the upper joint limit ( see above ). See See safety limits for more details.
*
* **k_position** ( optional, defaults to 0 ) - An attribute specifying the relation between position
* and velocity limits. See See safety limits for more details.
*
* k_velocity ( required ) - An attribute specifying the relation between effort and velocity limits.
* See See safety limits for more details.
*/
struct SoftJointLimits
{
SoftJointLimits( )
: min_position( std::numeric_limits<double>::quiet_NaN( ) ),
max_position( std::numeric_limits<double>::quiet_NaN( ) ),
k_position( std::numeric_limits<double>::quiet_NaN( ) ),
k_velocity( std::numeric_limits<double>::quiet_NaN( ) )
{
}
double min_position;
double max_position;
double k_position;
double k_velocity;
121 std::string to_string( )
{
std::stringstream ss_output;
ss_output << " soft position limits: "
<< "[" << min_position << ", " << max_position << "]\n";
ss_output << " k-position: "
<< "[" << k_position << "]\n";
ss_output << " k-velocity: "
<< "[" << k_velocity << "]\n";
return ss_output.str( );
}
};
} // namespace joint_limits
#endif // JOINT_LIMITS__JOINT_LIMITS_HPP_
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/// \author Adolfo Rodriguez Tsouroukdissian
#ifndef JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_
#define JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_
#include <limits>
#include <string>
#include "joint_limits/joint_limits.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
namespace // utilities
{
/// Declare and initialize a parameter with a type.
/**
*
* Wrapper function for templated node's declare_parameter( ) which checks if
* parameter is already declared.
* For use in all components that inherit from ControllerInterface
*/
template <typename ParameterT>
37 auto auto_declare(
const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf,
const std::string & name, const ParameterT & default_value )
{
if ( !param_itf->has_parameter( name ) )
{
auto param_default_value = rclcpp::ParameterValue( default_value );
param_itf->declare_parameter( name, param_default_value );
}
return param_itf->get_parameter( name ).get_value<ParameterT>( );
}
} // namespace
namespace joint_limits
{
/**
* Declare JointLimits and SoftJointLimits parameters for joint with \p joint_name using node
* parameters interface \p param_itf.
*
* The following parameter structure is declared with base name `joint_limits.joint_name`:
* \code
* has_position_limits: bool
* min_position: double
* max_position: double
* has_velocity_limits: bool
* max_velocity: double
* has_acceleration_limits: bool
* max_acceleration: double
* has_jerk_limits: bool
* max_jerk: double
* has_effort_limits: bool
* max_effort: double
* angle_wraparound: bool
* has_soft_limits: bool
* k_position: double
* k_velocity: double
* soft_lower_limit: double
* soft_upper_limit: double
* \endcode
*
* \param[in] joint_name name of the joint for which parameters will be declared.
* \param[in] param_itf node parameters interface object to access parameters.
* \param[in] logging_itf node logging interface to log if error happens.
*
* \returns True if parameters are successfully declared, false otherwise.
*/
83 inline bool declare_parameters(
const std::string & joint_name,
const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf,
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf )
{
const std::string param_base_name = "joint_limits." + joint_name;
try
{
auto_declare<bool>( param_itf, param_base_name + ".has_position_limits", false );
auto_declare<double>(
param_itf, param_base_name + ".min_position", std::numeric_limits<double>::quiet_NaN( ) );
auto_declare<double>(
param_itf, param_base_name + ".max_position", std::numeric_limits<double>::quiet_NaN( ) );
auto_declare<bool>( param_itf, param_base_name + ".has_velocity_limits", false );
auto_declare<double>(
param_itf, param_base_name + ".min_velocity", std::numeric_limits<double>::quiet_NaN( ) );
auto_declare<double>(
param_itf, param_base_name + ".max_velocity", std::numeric_limits<double>::quiet_NaN( ) );
auto_declare<bool>( param_itf, param_base_name + ".has_acceleration_limits", false );
auto_declare<double>(
param_itf, param_base_name + ".max_acceleration", std::numeric_limits<double>::quiet_NaN( ) );
auto_declare<bool>( param_itf, param_base_name + ".has_jerk_limits", false );
auto_declare<double>(
param_itf, param_base_name + ".max_jerk", std::numeric_limits<double>::quiet_NaN( ) );
auto_declare<bool>( param_itf, param_base_name + ".has_effort_limits", false );
auto_declare<double>(
param_itf, param_base_name + ".max_effort", std::numeric_limits<double>::quiet_NaN( ) );
auto_declare<bool>( param_itf, param_base_name + ".angle_wraparound", false );
auto_declare<bool>( param_itf, param_base_name + ".has_soft_limits", false );
auto_declare<double>(
param_itf, param_base_name + ".k_position", std::numeric_limits<double>::quiet_NaN( ) );
auto_declare<double>(
param_itf, param_base_name + ".k_velocity", std::numeric_limits<double>::quiet_NaN( ) );
auto_declare<double>(
param_itf, param_base_name + ".soft_lower_limit", std::numeric_limits<double>::quiet_NaN( ) );
auto_declare<double>(
param_itf, param_base_name + ".soft_upper_limit", std::numeric_limits<double>::quiet_NaN( ) );
}
catch ( const std::exception & ex )
{
RCLCPP_ERROR( logging_itf->get_logger( ), "%s", ex.what( ) );
return false;
}
return true;
}
/**
* Declare JointLimits and SoftJointLimits parameters for joint with \p joint_name for the \p node
* object.
* This is a convenience function.
* For parameters structure see the underlying `declare_parameters` function.
*
* \param[in] joint_name name of the joint for which parameters will be declared.
* \param[in] node node for parameters should be declared.
*
* \returns True if parameters are successfully declared, false otherwise.
*/
140 inline bool declare_parameters( const std::string & joint_name, const rclcpp::Node::SharedPtr & node )
{
return declare_parameters(
joint_name, node->get_node_parameters_interface( ), node->get_node_logging_interface( ) );
}
/**
* Declare JointLimits and SoftJointLimits parameters for joint with joint_name for the lifecycle_node
* object.
* This is a convenience function.
* For parameters structure see the underlying `declare_parameters` function.
*
* \param[in] joint_name name of the joint for which parameters will be declared.
* \param[in] lifecycle_node lifecycle node for parameters should be declared.
*
* \returns True if parameters are successfully declared, false otherwise.
*/
157 inline bool declare_parameters(
const std::string & joint_name, const rclcpp_lifecycle::LifecycleNode::SharedPtr & lifecycle_node )
{
return declare_parameters(
joint_name, lifecycle_node->get_node_parameters_interface( ),
lifecycle_node->get_node_logging_interface( ) );
}
/// Populate a JointLimits instance from the node parameters.
/**
* It is assumed that parameter structure is the following:
* \code
* has_position_limits: bool
* min_position: double
* max_position: double
* has_velocity_limits: bool
* max_velocity: double
* has_acceleration_limits: bool
* max_acceleration: double
* has_jerk_limits: bool
* max_jerk: double
* has_effort_limits: bool
* max_effort: double
* angle_wraparound: bool # will be ignored if there are position limits
* \endcode
*
* Unspecified parameters are not added to the joint limits specification.
* A specification in a yaml would look like this:
* \code
* <node_name>
* ros__parameters:
* joint_limits:
* foo_joint:
* has_position_limits: true
* min_position: 0.0
* max_position: 1.0
* has_velocity_limits: true
* max_velocity: 2.0
* has_acceleration_limits: true
* max_acceleration: 5.0
* has_jerk_limits: true
* max_jerk: 100.0
* has_effort_limits: true
* max_effort: 20.0
* bar_joint:
* has_position_limits: false # Continuous joint
* angle_wraparound: true # available only for continuous joints
* has_velocity_limits: true
* max_velocity: 4.0
* \endcode
*
* \param[in] joint_name Name of joint whose limits are to be fetched, e.g., "foo_joint".
* \param[in] param_itf node parameters interface of the node where parameters are specified.
* \param[in] logging_itf node logging interface to provide log errors.
* \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite
* existing values. Values in \p limits not specified in the parameter server remain unchanged.
*
* \returns True if a limits specification is found ( i.e., the \p joint_limits/joint_name parameter exists in \p node ), false otherwise.
*/
216 inline bool get_joint_limits(
const std::string & joint_name,
const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf,
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf,
JointLimits & limits )
{
const std::string param_base_name = "joint_limits." + joint_name;
try
{
if (
!param_itf->has_parameter( param_base_name + ".has_position_limits" ) &&
!param_itf->has_parameter( param_base_name + ".min_position" ) &&
!param_itf->has_parameter( param_base_name + ".max_position" ) &&
!param_itf->has_parameter( param_base_name + ".has_velocity_limits" ) &&
!param_itf->has_parameter( param_base_name + ".min_velocity" ) &&
!param_itf->has_parameter( param_base_name + ".max_velocity" ) &&
!param_itf->has_parameter( param_base_name + ".has_acceleration_limits" ) &&
!param_itf->has_parameter( param_base_name + ".max_acceleration" ) &&
!param_itf->has_parameter( param_base_name + ".has_jerk_limits" ) &&
!param_itf->has_parameter( param_base_name + ".max_jerk" ) &&
!param_itf->has_parameter( param_base_name + ".has_effort_limits" ) &&
!param_itf->has_parameter( param_base_name + ".max_effort" ) &&
!param_itf->has_parameter( param_base_name + ".angle_wraparound" ) &&
!param_itf->has_parameter( param_base_name + ".has_soft_limits" ) &&
!param_itf->has_parameter( param_base_name + ".k_position" ) &&
!param_itf->has_parameter( param_base_name + ".k_velocity" ) &&
!param_itf->has_parameter( param_base_name + ".soft_lower_limit" ) &&
!param_itf->has_parameter( param_base_name + ".soft_upper_limit" ) )
{
RCLCPP_ERROR(
logging_itf->get_logger( ),
"No joint limits specification found for joint '%s' in the parameter server "
"( param name: %s ).",
joint_name.c_str( ), param_base_name.c_str( ) );
return false;
}
}
catch ( const std::exception & ex )
{
RCLCPP_ERROR( logging_itf->get_logger( ), "%s", ex.what( ) );
return false;
}
// Position limits
if ( param_itf->has_parameter( param_base_name + ".has_position_limits" ) )
{
limits.has_position_limits =
param_itf->get_parameter( param_base_name + ".has_position_limits" ).as_bool( );
if (
limits.has_position_limits && param_itf->has_parameter( param_base_name + ".min_position" ) &&
param_itf->has_parameter( param_base_name + ".max_position" ) )
{
limits.min_position = param_itf->get_parameter( param_base_name + ".min_position" ).as_double( );
limits.max_position = param_itf->get_parameter( param_base_name + ".max_position" ).as_double( );
}
else
{
limits.has_position_limits = false;
}
if (
!limits.has_position_limits &&
param_itf->has_parameter( param_base_name + ".angle_wraparound" ) )
{
limits.angle_wraparound =
param_itf->get_parameter( param_base_name + ".angle_wraparound" ).as_bool( );
}
}
// Velocity limits
if ( param_itf->has_parameter( param_base_name + ".has_velocity_limits" ) )
{
limits.has_velocity_limits =
param_itf->get_parameter( param_base_name + ".has_velocity_limits" ).as_bool( );
if ( limits.has_velocity_limits && param_itf->has_parameter( param_base_name + ".max_velocity" ) )
{
limits.max_velocity = param_itf->get_parameter( param_base_name + ".max_velocity" ).as_double( );
}
else
{
limits.has_velocity_limits = false;
}
}
// Acceleration limits
if ( param_itf->has_parameter( param_base_name + ".has_acceleration_limits" ) )
{
limits.has_acceleration_limits =
param_itf->get_parameter( param_base_name + ".has_acceleration_limits" ).as_bool( );
if (
limits.has_acceleration_limits &&
param_itf->has_parameter( param_base_name + ".max_acceleration" ) )
{
limits.max_acceleration =
param_itf->get_parameter( param_base_name + ".max_acceleration" ).as_double( );
}
else
{
limits.has_acceleration_limits = false;
}
}
// Jerk limits
if ( param_itf->has_parameter( param_base_name + ".has_jerk_limits" ) )
{
limits.has_jerk_limits =
param_itf->get_parameter( param_base_name + ".has_jerk_limits" ).as_bool( );
if ( limits.has_jerk_limits && param_itf->has_parameter( param_base_name + ".max_jerk" ) )
{
limits.max_jerk = param_itf->get_parameter( param_base_name + ".max_jerk" ).as_double( );
}
else
{
limits.has_jerk_limits = false;
}
}
// Effort limits
if ( param_itf->has_parameter( param_base_name + ".has_effort_limits" ) )
{
limits.has_effort_limits =
param_itf->get_parameter( param_base_name + ".has_effort_limits" ).as_bool( );
if ( limits.has_effort_limits && param_itf->has_parameter( param_base_name + ".max_effort" ) )
{
limits.has_effort_limits = true;
limits.max_effort = param_itf->get_parameter( param_base_name + ".max_effort" ).as_double( );
}
else
{
limits.has_effort_limits = false;
}
}
return true;
}
/**
* Populate a JointLimits instance from the node parameters.
* This is a convenience function.
* For parameters structure see the underlying `get_joint_limits` function.
*
* \param[in] joint_name Name of joint whose limits are to be fetched.
* \param[in] node Node object for which parameters should be fetched.
* \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite
* existing values. Values in \p limits not specified in the parameter server remain unchanged.
*
* \returns True if a limits specification is found, false otherwise.
*/
364 inline bool get_joint_limits(
const std::string & joint_name, const rclcpp::Node::SharedPtr & node, JointLimits & limits )
{
return get_joint_limits(
joint_name, node->get_node_parameters_interface( ), node->get_node_logging_interface( ), limits );
}
/**
* Populate a JointLimits instance from the node parameters.
* This is a convenience function.
* For parameters structure see the underlying `get_joint_limits` function.
*
* \param[in] joint_name Name of joint whose limits are to be fetched.
* \param[in] lifecycle_node Lifecycle node object for which parameters should be fetched.
* \param[out] limits Where joint limit data gets written into. Limits specified in the parameter server will overwrite
* existing values. Values in \p limits not specified in the parameter server remain unchanged.
*
* \returns True if a limits specification is found, false otherwise.
*/
383 inline bool get_joint_limits(
const std::string & joint_name, const rclcpp_lifecycle::LifecycleNode::SharedPtr & lifecycle_node,
JointLimits & limits )
{
return get_joint_limits(
joint_name, lifecycle_node->get_node_parameters_interface( ),
lifecycle_node->get_node_logging_interface( ), limits );
}
/// Populate a SoftJointLimits instance from the ROS parameter server.
/**
* It is assumed that the parameter structure is the following:
* \code
* has_soft_limits: bool
* k_position: double
* k_velocity: double
* soft_lower_limit: double
* soft_upper_limit: double
* \endcode
*
* Only completely specified soft joint limits specifications will be considered valid.
* For example a valid yaml configuration would look like:
* \code
* <node_name>
* ros__parameters:
* joint_limits:
* foo_joint:
* soft_lower_limit: 0.0
* soft_upper_limit: 1.0
* k_position: 10.0
* k_velocity: 10.0
* \endcode
*
* \param[in] joint_name Name of joint whose limits are to be fetched, e.g., "foo_joint".
* \param[in] param_itf node parameters interface of the node where parameters are specified.
* \param[in] logging_itf node logging interface to provide log errors.
* \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will overwrite
* existing values.
* \return True if a complete soft limits specification is found ( ie. if all \p k_position, \p k_velocity, \p soft_lower_limit and
* \p soft_upper_limit exist in \p joint_limits/joint_name namespace ), false otherwise.
*/
424 inline bool get_joint_limits(
const std::string & joint_name,
const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf,
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf,
SoftJointLimits & soft_limits )
{
const std::string param_base_name = "joint_limits." + joint_name;
try
{
if (
!param_itf->has_parameter( param_base_name + ".has_soft_limits" ) &&
!param_itf->has_parameter( param_base_name + ".k_velocity" ) &&
!param_itf->has_parameter( param_base_name + ".k_position" ) &&
!param_itf->has_parameter( param_base_name + ".soft_lower_limit" ) &&
!param_itf->has_parameter( param_base_name + ".soft_upper_limit" ) )
{
RCLCPP_DEBUG(
logging_itf->get_logger( ),
"No soft joint limits specification found for joint '%s' in the parameter server "
"( param name: %s ).",
joint_name.c_str( ), param_base_name.c_str( ) );
return false;
}
}
catch ( const std::exception & ex )
{
RCLCPP_ERROR( logging_itf->get_logger( ), "%s", ex.what( ) );
return false;
}
// Override soft limits if complete specification is found
if ( param_itf->has_parameter( param_base_name + ".has_soft_limits" ) )
{
if (
param_itf->get_parameter( param_base_name + ".has_soft_limits" ).as_bool( ) &&
param_itf->has_parameter( param_base_name + ".k_position" ) &&
param_itf->has_parameter( param_base_name + ".k_velocity" ) &&
param_itf->has_parameter( param_base_name + ".soft_lower_limit" ) &&
param_itf->has_parameter( param_base_name + ".soft_upper_limit" ) )
{
soft_limits.k_position =
param_itf->get_parameter( param_base_name + ".k_position" ).as_double( );
soft_limits.k_velocity =
param_itf->get_parameter( param_base_name + ".k_velocity" ).as_double( );
soft_limits.min_position =
param_itf->get_parameter( param_base_name + ".soft_lower_limit" ).as_double( );
soft_limits.max_position =
param_itf->get_parameter( param_base_name + ".soft_upper_limit" ).as_double( );
return true;
}
}
return false;
}
/**
* Populate a JointLimits instance from the node parameters.
* This is a convenience function.
* For parameters structure see the underlying `get_joint_limits` function.
*
* \param[in] joint_name Name of joint whose limits are to be fetched.
* \param[in] node Node object for which parameters should be fetched.
* \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will overwrite
* existing values.
*
* \returns True if a soft limits specification is found, false otherwise.
*/
491 inline bool get_joint_limits(
const std::string & joint_name, const rclcpp::Node::SharedPtr & node,
SoftJointLimits & soft_limits )
{
return get_joint_limits(
joint_name, node->get_node_parameters_interface( ), node->get_node_logging_interface( ),
soft_limits );
}
/**
* Populate a JointLimits instance from the node parameters.
* This is a convenience function.
* For parameters structure see the underlying `get_joint_limits` function.
*
* \param[in] joint_name Name of joint whose limits are to be fetched.
* \param[in] lifecycle_node Lifecycle node object for which parameters should be fetched.
* \param[out] soft_limits Where soft joint limit data gets written into. Limits specified in the parameter server will overwrite
* existing values.
*
* \returns True if a soft limits specification is found, false otherwise.
*/
512 inline bool get_joint_limits(
const std::string & joint_name, const rclcpp_lifecycle::LifecycleNode::SharedPtr & lifecycle_node,
SoftJointLimits & soft_limits )
{
return get_joint_limits(
joint_name, lifecycle_node->get_node_parameters_interface( ),
lifecycle_node->get_node_logging_interface( ), soft_limits );
}
} // namespace joint_limits
#endif // JOINT_LIMITS__JOINT_LIMITS_ROSPARAM_HPP_
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/// \author Adolfo Rodriguez Tsouroukdissian
#include <memory>
#include "gtest/gtest.h"
#include "joint_limits/joint_limits_rosparam.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
25 class JointLimitsRosParamTest : public ::testing::Test
{
public:
28 void SetUp( )
{
rclcpp::NodeOptions node_options;
node_options.allow_undeclared_parameters( true ).automatically_declare_parameters_from_overrides(
true );
node_ = rclcpp::Node::make_shared( "JointLimitsRosparamTestNode", node_options );
}
37 void TearDown( ) { node_.reset( ); }
protected:
40 rclcpp::Node::SharedPtr node_;
};
43 TEST_F( JointLimitsRosParamTest, parse_joint_limits )
{
// Invalid specification
{
joint_limits::JointLimits limits;
// test default values
EXPECT_FALSE( limits.has_position_limits );
EXPECT_TRUE( std::isnan( limits.min_position ) );
EXPECT_TRUE( std::isnan( limits.max_position ) );
EXPECT_FALSE( limits.has_velocity_limits );
EXPECT_TRUE( std::isnan( limits.max_velocity ) );
EXPECT_FALSE( limits.has_acceleration_limits );
EXPECT_TRUE( std::isnan( limits.max_acceleration ) );
EXPECT_FALSE( limits.has_jerk_limits );
EXPECT_TRUE( std::isnan( limits.max_jerk ) );
EXPECT_FALSE( limits.has_effort_limits );
EXPECT_TRUE( std::isnan( limits.max_effort ) );
EXPECT_FALSE( limits.angle_wraparound );
// try to read limits for not-existing joints
EXPECT_FALSE( get_joint_limits(
"bad_joint", node_->get_node_parameters_interface( ), node_->get_node_logging_interface( ),
limits ) );
EXPECT_FALSE( get_joint_limits(
"unknown_joint", node_->get_node_parameters_interface( ), node_->get_node_logging_interface( ),
limits ) );
// default values should not change
EXPECT_FALSE( limits.has_position_limits );
EXPECT_TRUE( std::isnan( limits.min_position ) );
EXPECT_TRUE( std::isnan( limits.max_position ) );
EXPECT_FALSE( limits.has_velocity_limits );
EXPECT_TRUE( std::isnan( limits.max_velocity ) );
EXPECT_FALSE( limits.has_acceleration_limits );
EXPECT_TRUE( std::isnan( limits.max_acceleration ) );
EXPECT_FALSE( limits.has_jerk_limits );
EXPECT_TRUE( std::isnan( limits.max_jerk ) );
EXPECT_FALSE( limits.has_effort_limits );
EXPECT_TRUE( std::isnan( limits.max_effort ) );
EXPECT_FALSE( limits.angle_wraparound );
}
// Get full specification from parameter server
{
joint_limits::JointLimits limits;
EXPECT_TRUE( get_joint_limits(
"foo_joint", node_->get_node_parameters_interface( ), node_->get_node_logging_interface( ),
limits ) );
EXPECT_TRUE( limits.has_position_limits );
EXPECT_EQ( 0.0, limits.min_position );
EXPECT_EQ( 1.0, limits.max_position );
EXPECT_TRUE( limits.has_velocity_limits );
EXPECT_EQ( 2.0, limits.max_velocity );
EXPECT_TRUE( limits.has_acceleration_limits );
EXPECT_EQ( 5.0, limits.max_acceleration );
EXPECT_TRUE( limits.has_jerk_limits );
EXPECT_EQ( 100.0, limits.max_jerk );
EXPECT_TRUE( limits.has_effort_limits );
EXPECT_EQ( 20.0, limits.max_effort );
// parameters is 'true', but because there are position limits it is ignored
EXPECT_FALSE( limits.angle_wraparound );
}
// Specifying flags but not values should set nothing
{
joint_limits::JointLimits limits;
EXPECT_TRUE( get_joint_limits(
"yinfoo_joint", node_->get_node_parameters_interface( ), node_->get_node_logging_interface( ),
limits ) );
EXPECT_FALSE( limits.has_position_limits );
EXPECT_FALSE( limits.has_velocity_limits );
EXPECT_FALSE( limits.has_acceleration_limits );
EXPECT_FALSE( limits.has_jerk_limits );
EXPECT_FALSE( limits.has_effort_limits );
}
// Specifying values but not flags should set nothing
{
joint_limits::JointLimits limits;
EXPECT_TRUE( get_joint_limits(
"yangfoo_joint", node_->get_node_parameters_interface( ), node_->get_node_logging_interface( ),
limits ) );
EXPECT_FALSE( limits.has_position_limits );
EXPECT_FALSE( limits.has_velocity_limits );
EXPECT_FALSE( limits.has_acceleration_limits );
EXPECT_FALSE( limits.has_jerk_limits );
EXPECT_FALSE( limits.has_effort_limits );
}
// Disable already set values
{
joint_limits::JointLimits limits;
EXPECT_TRUE( get_joint_limits(
"foo_joint", node_->get_node_parameters_interface( ), node_->get_node_logging_interface( ),
limits ) );
EXPECT_TRUE( limits.has_position_limits );
EXPECT_TRUE( limits.has_velocity_limits );
EXPECT_TRUE( limits.has_acceleration_limits );
EXPECT_TRUE( limits.has_jerk_limits );
EXPECT_TRUE( limits.has_effort_limits );
EXPECT_TRUE( get_joint_limits(
"antifoo_joint", node_->get_node_parameters_interface( ), node_->get_node_logging_interface( ),
limits ) );
EXPECT_FALSE( limits.has_position_limits );
EXPECT_FALSE( limits.has_velocity_limits );
EXPECT_FALSE( limits.has_acceleration_limits );
EXPECT_FALSE( limits.has_jerk_limits );
EXPECT_FALSE( limits.has_effort_limits );
EXPECT_TRUE( limits.angle_wraparound );
}
// Incomplete position limits specification does not get loaded
{
joint_limits::JointLimits limits;
EXPECT_TRUE( get_joint_limits(
"baz_joint", node_->get_node_parameters_interface( ), node_->get_node_logging_interface( ),
limits ) );
EXPECT_FALSE( limits.has_position_limits );
EXPECT_TRUE( std::isnan( limits.min_position ) );
EXPECT_TRUE( std::isnan( limits.max_position ) );
}
// Override only one field, leave all others unchanged
{
joint_limits::JointLimits limits;
EXPECT_TRUE( get_joint_limits(
"bar_joint", node_->get_node_parameters_interface( ), node_->get_node_logging_interface( ),
limits ) );
EXPECT_FALSE( limits.has_position_limits );
EXPECT_TRUE( std::isnan( limits.min_position ) );
EXPECT_TRUE( std::isnan( limits.max_position ) );
// Max velocity is overridden
EXPECT_TRUE( limits.has_velocity_limits );
EXPECT_EQ( 2.0, limits.max_velocity );
EXPECT_FALSE( limits.has_acceleration_limits );
EXPECT_TRUE( std::isnan( limits.max_acceleration ) );
EXPECT_FALSE( limits.has_jerk_limits );
EXPECT_TRUE( std::isnan( limits.max_jerk ) );
EXPECT_FALSE( limits.has_effort_limits );
EXPECT_TRUE( std::isnan( limits.max_effort ) );
}
}
202 TEST_F( JointLimitsRosParamTest, parse_soft_joint_limits )
{
// Invalid specification
{
joint_limits::SoftJointLimits soft_limits;
// test default values
EXPECT_TRUE( std::isnan( soft_limits.min_position ) );
EXPECT_TRUE( std::isnan( soft_limits.max_position ) );
EXPECT_TRUE( std::isnan( soft_limits.k_position ) );
EXPECT_TRUE( std::isnan( soft_limits.k_velocity ) );
// try to read limits for not-existing joints
EXPECT_FALSE( get_joint_limits(
"bad_joint", node_->get_node_parameters_interface( ), node_->get_node_logging_interface( ),
soft_limits ) );
EXPECT_FALSE( get_joint_limits(
"unknown_joint", node_->get_node_parameters_interface( ), node_->get_node_logging_interface( ),
soft_limits ) );
// default values should not change
EXPECT_TRUE( std::isnan( soft_limits.min_position ) );
EXPECT_TRUE( std::isnan( soft_limits.max_position ) );
EXPECT_TRUE( std::isnan( soft_limits.k_position ) );
EXPECT_TRUE( std::isnan( soft_limits.k_velocity ) );
}
// Get full specification from parameter server
{
joint_limits::SoftJointLimits soft_limits;
EXPECT_TRUE( get_joint_limits(
"foo_joint", node_->get_node_parameters_interface( ), node_->get_node_logging_interface( ),
soft_limits ) );
EXPECT_EQ( 10.0, soft_limits.k_position );
EXPECT_EQ( 20.0, soft_limits.k_velocity );
EXPECT_EQ( 0.1, soft_limits.min_position );
EXPECT_EQ( 0.9, soft_limits.max_position );
}
// Skip parsing soft limits if has_soft_limits is false
{
joint_limits::SoftJointLimits soft_limits;
EXPECT_FALSE( get_joint_limits(
"foobar_joint", node_->get_node_parameters_interface( ), node_->get_node_logging_interface( ),
soft_limits ) );
EXPECT_TRUE( std::isnan( soft_limits.min_position ) );
EXPECT_TRUE( std::isnan( soft_limits.max_position ) );
EXPECT_TRUE( std::isnan( soft_limits.k_position ) );
EXPECT_TRUE( std::isnan( soft_limits.k_velocity ) );
}
// Incomplete soft limits specification does not get loaded
{
joint_limits::SoftJointLimits soft_limits;
EXPECT_FALSE( get_joint_limits(
"barbaz_joint", node_->get_node_parameters_interface( ), node_->get_node_logging_interface( ),
soft_limits ) );
EXPECT_TRUE( std::isnan( soft_limits.min_position ) );
EXPECT_TRUE( std::isnan( soft_limits.max_position ) );
EXPECT_TRUE( std::isnan( soft_limits.k_position ) );
EXPECT_TRUE( std::isnan( soft_limits.k_velocity ) );
}
}
267 class JointLimitsUndeclaredRosParamTest : public ::testing::Test
{
public:
270 void SetUp( ) { node_ = rclcpp::Node::make_shared( "JointLimitsRosparamTestNode" ); }
272 void TearDown( ) { node_.reset( ); }
protected:
275 rclcpp::Node::SharedPtr node_;
};
278 class JointLimitsLifecycleNodeUndeclaredRosParamTest : public ::testing::Test
{
public:
281 void SetUp( )
{
lifecycle_node_ = rclcpp_lifecycle::LifecycleNode::make_shared( "JointLimitsRosparamTestNode" );
}
286 void TearDown( ) { lifecycle_node_.reset( ); }
protected:
289 rclcpp_lifecycle::LifecycleNode::SharedPtr lifecycle_node_;
};
292 TEST_F( JointLimitsUndeclaredRosParamTest, parse_declared_joint_limits_node )
{
// Get full specification from parameter server - no need to test logic
{
joint_limits::JointLimits limits;
// try to read limits for not-existing joints
EXPECT_FALSE( get_joint_limits( "bad_joint", node_, limits ) );
EXPECT_FALSE( get_joint_limits( "unknown_joint", node_, limits ) );
// try to read existing but undeclared joint
EXPECT_FALSE( get_joint_limits( "foo_joint", node_, limits ) );
// declare parameters
EXPECT_TRUE( joint_limits::declare_parameters( "foo_joint", node_ ) );
// now should be successful
EXPECT_TRUE( get_joint_limits( "foo_joint", node_, limits ) );
EXPECT_TRUE( limits.has_position_limits );
EXPECT_EQ( 0.0, limits.min_position );
EXPECT_EQ( 1.0, limits.max_position );
EXPECT_TRUE( limits.has_velocity_limits );
EXPECT_EQ( 2.0, limits.max_velocity );
EXPECT_TRUE( limits.has_acceleration_limits );
EXPECT_EQ( 5.0, limits.max_acceleration );
EXPECT_TRUE( limits.has_jerk_limits );
EXPECT_EQ( 100.0, limits.max_jerk );
EXPECT_TRUE( limits.has_effort_limits );
EXPECT_EQ( 20.0, limits.max_effort );
// parameters is 'true', but because there are position limits it is ignored
EXPECT_FALSE( limits.angle_wraparound );
}
}
331 TEST_F( JointLimitsLifecycleNodeUndeclaredRosParamTest, parse_declared_joint_limits_lifecycle_node )
{
// Get full specification from parameter server - no need to test logic
{
joint_limits::JointLimits limits;
// try to read limits for not-existing joints
EXPECT_FALSE( get_joint_limits( "bad_joint", lifecycle_node_, limits ) );
EXPECT_FALSE( get_joint_limits( "unknown_joint", lifecycle_node_, limits ) );
// try to read existing but undeclared joint
EXPECT_FALSE( get_joint_limits( "foo_joint", lifecycle_node_, limits ) );
// declare parameters
EXPECT_TRUE( joint_limits::declare_parameters( "foo_joint", lifecycle_node_ ) );
// now should be successful
EXPECT_TRUE( get_joint_limits( "foo_joint", lifecycle_node_, limits ) );
EXPECT_TRUE( limits.has_position_limits );
EXPECT_EQ( 0.0, limits.min_position );
EXPECT_EQ( 1.0, limits.max_position );
EXPECT_TRUE( limits.has_velocity_limits );
EXPECT_EQ( 2.0, limits.max_velocity );
EXPECT_TRUE( limits.has_acceleration_limits );
EXPECT_EQ( 5.0, limits.max_acceleration );
EXPECT_TRUE( limits.has_jerk_limits );
EXPECT_EQ( 100.0, limits.max_jerk );
EXPECT_TRUE( limits.has_effort_limits );
EXPECT_EQ( 20.0, limits.max_effort );
// parameters is 'true', but because there are position limits it is ignored
EXPECT_FALSE( limits.angle_wraparound );
}
}
370 TEST_F( JointLimitsUndeclaredRosParamTest, parse_declared_soft_joint_limits_node )
{
// Get full specification from parameter server - no need to test logic
{
joint_limits::SoftJointLimits soft_limits;
// try to read existing but undeclared joint
EXPECT_FALSE( get_joint_limits( "foo_joint", node_, soft_limits ) );
// declare parameters
EXPECT_TRUE( joint_limits::declare_parameters( "foo_joint", node_ ) );
// now should be successful
EXPECT_TRUE( get_joint_limits( "foo_joint", node_, soft_limits ) );
EXPECT_EQ( 10.0, soft_limits.k_position );
EXPECT_EQ( 20.0, soft_limits.k_velocity );
EXPECT_EQ( 0.1, soft_limits.min_position );
EXPECT_EQ( 0.9, soft_limits.max_position );
}
}
391 TEST_F(
JointLimitsLifecycleNodeUndeclaredRosParamTest, parse_declared_soft_joint_limits_lifecycle_node )
{
// Get full specification from parameter server - no need to test logic
{
joint_limits::SoftJointLimits soft_limits;
// try to read existing but undeclared joint
EXPECT_FALSE( get_joint_limits( "foo_joint", lifecycle_node_, soft_limits ) );
// declare parameters
EXPECT_TRUE( joint_limits::declare_parameters( "foo_joint", lifecycle_node_ ) );
// now should be successful
EXPECT_TRUE( get_joint_limits( "foo_joint", lifecycle_node_, soft_limits ) );
EXPECT_EQ( 10.0, soft_limits.k_position );
EXPECT_EQ( 20.0, soft_limits.k_velocity );
EXPECT_EQ( 0.1, soft_limits.min_position );
EXPECT_EQ( 0.9, soft_limits.max_position );
}
}
413 int main( int argc, char ** argv )
{
rclcpp::init( argc, argv );
testing::InitGoogleTest( &argc, argv );
int ret = RUN_ALL_TESTS( );
rclcpp::shutdown( );
return ret;
}
// Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/// \author Adolfo Rodriguez Tsouroukdissian
#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_
#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_
#include <hardware_interface/joint_handle.hpp>
#include <hardware_interface/types/hardware_interface_type_values.hpp>
#include <rclcpp/duration.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rcppmath/clamp.hpp>
#include <algorithm>
#include <cassert>
#include <cmath>
#include <limits>
#include <memory>
#include <string>
#include "joint_limits_interface/joint_limits.hpp"
#include "joint_limits_interface/joint_limits_interface_exception.hpp"
namespace joint_limits_interface
{
/**
* The base class of limit handles for enforcing position, velocity, and effort limits of
* an effort-controlled joint.
*/
44 class JointLimitHandle
{
public:
JointLimitHandle( )
: prev_pos_( std::numeric_limits<double>::quiet_NaN( ) ),
prev_vel_( 0.0 ),
jposh_( hardware_interface::HW_IF_POSITION ),
jvelh_( hardware_interface::HW_IF_VELOCITY ),
jcmdh_( "position_command" )
{
}
JointLimitHandle(
const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jcmdh,
const JointLimits & limits )
: jposh_( jposh ),
jvelh_( hardware_interface::HW_IF_VELOCITY ),
jcmdh_( jcmdh ),
limits_( limits ),
prev_pos_( std::numeric_limits<double>::quiet_NaN( ) ),
64 prev_vel_( 0.0 )
{
}
68 JointLimitHandle(
const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jvelh,
const hardware_interface::JointHandle & jcmdh, const JointLimits & limits )
: jposh_( jposh ),
jvelh_( jvelh ),
jcmdh_( jcmdh ),
limits_( limits ),
prev_pos_( std::numeric_limits<double>::quiet_NaN( ) ),
prev_vel_( 0.0 )
{
}
/// \return Joint name.
std::string get_name( ) const
{
return jposh_ ? jposh_.get_name( )
: jvelh_ ? jvelh_.get_name( )
: jcmdh_ ? jcmdh_.get_name( )
: std::string( );
}
/// Sub-class implementation of limit enforcing policy.
virtual void enforce_limits( const rclcpp::Duration & period ) = 0;
/// Clear stored state, causing it to reset next iteration.
virtual void reset( )
{
prev_pos_ = std::numeric_limits<double>::quiet_NaN( );
prev_vel_ = 0.0;
}
protected:
hardware_interface::JointHandle jposh_;
hardware_interface::JointHandle jvelh_;
hardware_interface::JointHandle jcmdh_;
joint_limits_interface::JointLimits limits_;
// stored state - track position and velocity of last update
double prev_pos_;
double prev_vel_;
/// Return velocity for limit calculations.
/**
* @param period Time since last measurement
* @return the velocity, from state if available, otherwise from previous position history.
*/
double get_velocity( const rclcpp::Duration & period ) const
{
// if we have a handle to a velocity state we can directly return state velocity
// otherwise we will estimate velocity from previous position ( command or state )
return jvelh_ ? jvelh_.get_value( ) : ( jposh_.get_value( ) - prev_pos_ ) / period.seconds( );
}
};
/** The base class of limit handles for enforcing position, velocity, and effort limits of
* an effort-controlled joint that has soft-limits.
*/
class JointSoftLimitsHandle : public JointLimitHandle
{
public:
JointSoftLimitsHandle( ) {}
JointSoftLimitsHandle(
const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jcmdh,
const JointLimits & limits, const SoftJointLimits & soft_limits )
: JointLimitHandle( jposh, jcmdh, limits ), soft_limits_( soft_limits )
{
}
JointSoftLimitsHandle(
const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jvelh,
const hardware_interface::JointHandle & jcmdh, const JointLimits & limits,
const SoftJointLimits & soft_limits )
: JointLimitHandle( jposh, jvelh, jcmdh, limits ), soft_limits_( soft_limits )
{
}
protected:
joint_limits_interface::SoftJointLimits soft_limits_;
};
/** A handle used to enforce position and velocity limits of a position-controlled joint that does not have
soft limits. */
class PositionJointSaturationHandle : public JointLimitHandle
{
public:
PositionJointSaturationHandle( ) {}
PositionJointSaturationHandle(
const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jcmdh,
const JointLimits & limits )
: JointLimitHandle( jposh, jcmdh, limits )
{
if ( limits_.has_position_limits )
{
min_pos_limit_ = limits_.min_position;
max_pos_limit_ = limits_.max_position;
}
else
{
min_pos_limit_ = -std::numeric_limits<double>::max( );
max_pos_limit_ = std::numeric_limits<double>::max( );
}
}
/// Enforce position and velocity limits for a joint that is not subject to soft limits.
/**
* \param[in] period Control period.
*/
void enforce_limits( const rclcpp::Duration & period )
{
if ( std::isnan( prev_pos_ ) )
{
prev_pos_ = jposh_.get_value( );
}
double min_pos, max_pos;
if ( limits_.has_velocity_limits )
{
// enforce velocity limits
// set constraints on where the position can be based on the
// max velocity times seconds since last update
const double delta_pos = limits_.max_velocity * period.seconds( );
min_pos = std::max( prev_pos_ - delta_pos, min_pos_limit_ );
max_pos = std::min( prev_pos_ + delta_pos, max_pos_limit_ );
}
else
{
// no velocity limit, so position is simply limited to set extents ( our imposed soft limits )
min_pos = min_pos_limit_;
max_pos = max_pos_limit_;
}
// clamp command position to our computed min/max position
const double cmd = rcppmath::clamp( jcmdh_.get_value( ), min_pos, max_pos );
jcmdh_.set_value( cmd );
prev_pos_ = cmd;
}
private:
double min_pos_limit_, max_pos_limit_;
};
/// A handle used to enforce position and velocity limits of a position-controlled joint.
/**
* This class implements a very simple position and velocity limits enforcing policy, and tries to impose the least
* amount of requisites on the underlying hardware platform.
* This lowers considerably the entry barrier to use it, but also implies some limitations.
*
* <b>Requisites</b>
* - Position ( for non-continuous joints ) and velocity limits specification.
* - Soft limits specification. The \c k_velocity parameter is \e not used.
*
* <b>Open loop nature</b>
*
* Joint position and velocity limits are enforced in an open-loop fashion, that is, the command is checked for
* validity without relying on the actual position/velocity values.
*
* - Actual position values are \e not used because in some platforms there might be a substantial lag
* between sending a command and executing it ( propagate command to hardware, reach control objective,
* read from hardware ).
*
* - Actual velocity values are \e not used because of the above reason, and because some platforms might not expose
* trustworthy velocity measurements, or none at all.
*
* The downside of the open loop behavior is that velocity limits will not be enforced when recovering from large
* position tracking errors. Only the command is guaranteed to comply with the limits specification.
*
* \note: This handle type is \e stateful, ie. it stores the previous position command to estimate the command
* velocity.
*/
// TODO( anyone ): Leverage %Reflexxes Type II library for acceleration limits handling?
class PositionJointSoftLimitsHandle : public JointSoftLimitsHandle
{
public:
PositionJointSoftLimitsHandle( ) {}
PositionJointSoftLimitsHandle(
const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jcmdh,
const joint_limits_interface::JointLimits & limits,
const joint_limits_interface::SoftJointLimits & soft_limits )
: JointSoftLimitsHandle( jposh, jcmdh, limits, soft_limits )
{
if ( !limits.has_velocity_limits )
{
throw joint_limits_interface::JointLimitsInterfaceException(
"Cannot enforce limits for joint '" + get_name( ) +
"'. It has no velocity limits specification." );
}
}
/// Enforce position and velocity limits for a joint subject to soft limits.
/**
* If the joint has no position limits ( eg. a continuous joint ), only velocity limits will be
* enforced.
* \param[in] period Control period.
*/
void enforce_limits( const rclcpp::Duration & period ) override
{
assert( period.seconds( ) > 0.0 );
// Current position
if ( std::isnan( prev_pos_ ) )
{
// Happens only once at initialization
prev_pos_ = jposh_.get_value( );
}
const double pos = prev_pos_;
// Velocity bounds
double soft_min_vel;
double soft_max_vel;
if ( limits_.has_position_limits )
{
// Velocity bounds depend on the velocity limit and the proximity to the position limit
soft_min_vel = rcppmath::clamp(
-soft_limits_.k_position * ( pos - soft_limits_.min_position ), -limits_.max_velocity,
limits_.max_velocity );
soft_max_vel = rcppmath::clamp(
-soft_limits_.k_position * ( pos - soft_limits_.max_position ), -limits_.max_velocity,
limits_.max_velocity );
}
else
{
// No position limits, eg. continuous joints
soft_min_vel = -limits_.max_velocity;
soft_max_vel = limits_.max_velocity;
}
// Position bounds
const double dt = period.seconds( );
double pos_low = pos + soft_min_vel * dt;
double pos_high = pos + soft_max_vel * dt;
if ( limits_.has_position_limits )
{
// This extra measure safeguards against pathological cases, like when the soft limit lies
// beyond the hard limit
pos_low = std::max( pos_low, limits_.min_position );
pos_high = std::min( pos_high, limits_.max_position );
}
// Saturate position command according to bounds
const double pos_cmd = rcppmath::clamp( jcmdh_.get_value( ), pos_low, pos_high );
jcmdh_.set_value( pos_cmd );
// Cache variables
// todo: shouldn't this just be pos_cmd? why call into the command handle to
// get what we have in the above line?
prev_pos_ = jcmdh_.get_value( );
}
};
/**
* A handle used to enforce position, velocity, and effort limits of an effort-controlled
* joint that does not have soft limits.
*/
class EffortJointSaturationHandle : public JointLimitHandle
{
public:
EffortJointSaturationHandle( ) {}
EffortJointSaturationHandle(
const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jvelh,
const hardware_interface::JointHandle & jcmdh,
const joint_limits_interface::JointLimits & limits )
: JointLimitHandle( jposh, jvelh, jcmdh, limits )
{
if ( !limits.has_velocity_limits )
{
throw joint_limits_interface::JointLimitsInterfaceException(
"Cannot enforce limits for joint '" + get_name( ) +
"'. It has no velocity limits specification." );
}
if ( !limits.has_effort_limits )
{
throw joint_limits_interface::JointLimitsInterfaceException(
"Cannot enforce limits for joint '" + get_name( ) +
"'. It has no efforts limits specification." );
}
}
EffortJointSaturationHandle(
const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jcmdh,
const joint_limits_interface::JointLimits & limits )
: EffortJointSaturationHandle(
jposh, hardware_interface::JointHandle( hardware_interface::HW_IF_VELOCITY ), jcmdh, limits )
{
}
/**
* Enforce position, velocity, and effort limits for a joint that is not subject
* to soft limits.
*
* \param[in] period Control period.
*/
void enforce_limits( const rclcpp::Duration & period ) override
{
double min_eff = -limits_.max_effort;
double max_eff = limits_.max_effort;
if ( limits_.has_position_limits )
{
const double pos = jposh_.get_value( );
if ( pos < limits_.min_position )
{
min_eff = 0.0;
}
else if ( pos > limits_.max_position )
{
max_eff = 0.0;
}
}
const double vel = get_velocity( period );
if ( vel < -limits_.max_velocity )
{
min_eff = 0.0;
}
else if ( vel > limits_.max_velocity )
{
max_eff = 0.0;
}
double clamped = rcppmath::clamp( jcmdh_.get_value( ), min_eff, max_eff );
jcmdh_.set_value( clamped );
}
};
/// A handle used to enforce position, velocity and effort limits of an effort-controlled joint.
// TODO( anyone ): This class is untested!. Update unit tests accordingly.
class EffortJointSoftLimitsHandle : public JointSoftLimitsHandle
{
public:
EffortJointSoftLimitsHandle( ) {}
EffortJointSoftLimitsHandle(
const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jvelh,
const hardware_interface::JointHandle & jcmdh,
const joint_limits_interface::JointLimits & limits,
const joint_limits_interface::SoftJointLimits & soft_limits )
: JointSoftLimitsHandle( jposh, jvelh, jcmdh, limits, soft_limits )
{
if ( !limits.has_velocity_limits )
{
throw joint_limits_interface::JointLimitsInterfaceException(
"Cannot enforce limits for joint '" + get_name( ) +
"'. It has no velocity limits specification." );
}
if ( !limits.has_effort_limits )
{
throw joint_limits_interface::JointLimitsInterfaceException(
"Cannot enforce limits for joint '" + get_name( ) +
"'. It has no effort limits specification." );
}
}
EffortJointSoftLimitsHandle(
const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jcmdh,
const joint_limits_interface::JointLimits & limits,
const joint_limits_interface::SoftJointLimits & soft_limits )
: EffortJointSoftLimitsHandle(
jposh, hardware_interface::JointHandle( hardware_interface::HW_IF_VELOCITY ), jcmdh, limits,
soft_limits )
{
}
/// Enforce position, velocity and effort limits for a joint subject to soft limits.
/**
* If the joint has no position limits ( eg. a continuous joint ), only velocity and effort limits
* will be enforced.
*
* \param[in] period Control period.
*/
void enforce_limits( const rclcpp::Duration & period ) override
{
// Current state
const double pos = jposh_.get_value( );
const double vel = get_velocity( period );
// Velocity bounds
double soft_min_vel;
double soft_max_vel;
if ( limits_.has_position_limits )
{
// Velocity bounds depend on the velocity limit and the proximity to the position limit
soft_min_vel = rcppmath::clamp(
-soft_limits_.k_position * ( pos - soft_limits_.min_position ), -limits_.max_velocity,
limits_.max_velocity );
soft_max_vel = rcppmath::clamp(
-soft_limits_.k_position * ( pos - soft_limits_.max_position ), -limits_.max_velocity,
limits_.max_velocity );
}
else
{
// No position limits, eg. continuous joints
soft_min_vel = -limits_.max_velocity;
soft_max_vel = limits_.max_velocity;
}
// Effort bounds depend on the velocity and effort bounds
const double soft_min_eff = rcppmath::clamp(
-soft_limits_.k_velocity * ( vel - soft_min_vel ), -limits_.max_effort, limits_.max_effort );
const double soft_max_eff = rcppmath::clamp(
-soft_limits_.k_velocity * ( vel - soft_max_vel ), -limits_.max_effort, limits_.max_effort );
// Saturate effort command according to bounds
const double eff_cmd = rcppmath::clamp( jcmdh_.get_value( ), soft_min_eff, soft_max_eff );
jcmdh_.set_value( eff_cmd );
}
};
/// A handle used to enforce velocity and acceleration limits of a velocity-controlled joint.
class VelocityJointSaturationHandle : public JointLimitHandle
{
public:
VelocityJointSaturationHandle( ) {}
VelocityJointSaturationHandle(
const hardware_interface::JointHandle & jvelh, // currently unused
const hardware_interface::JointHandle & jcmdh,
const joint_limits_interface::JointLimits & limits )
: JointLimitHandle(
hardware_interface::JointHandle( hardware_interface::HW_IF_POSITION ), jvelh, jcmdh, limits )
{
if ( !limits.has_velocity_limits )
{
throw joint_limits_interface::JointLimitsInterfaceException(
"Cannot enforce limits for joint '" + get_name( ) +
"'. It has no velocity limits specification." );
}
}
VelocityJointSaturationHandle(
const hardware_interface::JointHandle & jcmdh,
const joint_limits_interface::JointLimits & limits )
: JointLimitHandle(
hardware_interface::JointHandle( hardware_interface::HW_IF_POSITION ),
hardware_interface::JointHandle( hardware_interface::HW_IF_VELOCITY ), jcmdh, limits )
{
if ( !limits.has_velocity_limits )
{
throw joint_limits_interface::JointLimitsInterfaceException(
"Cannot enforce limits for joint '" + get_name( ) +
"'. It has no velocity limits specification." );
}
}
/// Enforce joint velocity and acceleration limits.
/**
* \param[in] period Control period.
*/
void enforce_limits( const rclcpp::Duration & period ) override
{
// Velocity bounds
double vel_low;
double vel_high;
if ( limits_.has_acceleration_limits )
{
assert( period.seconds( ) > 0.0 );
const double dt = period.seconds( );
vel_low = std::max( prev_vel_ - limits_.max_acceleration * dt, -limits_.max_velocity );
vel_high = std::min( prev_vel_ + limits_.max_acceleration * dt, limits_.max_velocity );
}
else
{
vel_low = -limits_.max_velocity;
vel_high = limits_.max_velocity;
}
// Saturate velocity command according to limits
const double vel_cmd = rcppmath::clamp( jcmdh_.get_value( ), vel_low, vel_high );
jcmdh_.set_value( vel_cmd );
// Cache variables
prev_vel_ = jcmdh_.get_value( );
}
};
/**
* A handle used to enforce position, velocity, and acceleration limits of a
* velocity-controlled joint.
*/
class VelocityJointSoftLimitsHandle : public JointSoftLimitsHandle
{
public:
VelocityJointSoftLimitsHandle( ) {}
VelocityJointSoftLimitsHandle(
const hardware_interface::JointHandle & jposh, const hardware_interface::JointHandle & jvelh,
const hardware_interface::JointHandle & jcmdh,
const joint_limits_interface::JointLimits & limits,
const joint_limits_interface::SoftJointLimits & soft_limits )
: JointSoftLimitsHandle( jposh, jvelh, jcmdh, limits, soft_limits )
{
if ( limits.has_velocity_limits )
{
max_vel_limit_ = limits.max_velocity;
}
else
{
max_vel_limit_ = std::numeric_limits<double>::max( );
}
}
/**
* Enforce position, velocity, and acceleration limits for a velocity-controlled joint
* subject to soft limits.
*
* \param[in] period Control period.
*/
void enforce_limits( const rclcpp::Duration & period )
{
double min_vel, max_vel;
if ( limits_.has_position_limits )
{
// Velocity bounds depend on the velocity limit and the proximity to the position limit.
const double pos = jposh_.get_value( );
min_vel = rcppmath::clamp(
-soft_limits_.k_position * ( pos - soft_limits_.min_position ), -max_vel_limit_,
max_vel_limit_ );
max_vel = rcppmath::clamp(
-soft_limits_.k_position * ( pos - soft_limits_.max_position ), -max_vel_limit_,
max_vel_limit_ );
}
else
{
min_vel = -max_vel_limit_;
max_vel = max_vel_limit_;
}
if ( limits_.has_acceleration_limits )
{
const double vel = get_velocity( period );
const double delta_t = period.seconds( );
min_vel = std::max( vel - limits_.max_acceleration * delta_t, min_vel );
max_vel = std::min( vel + limits_.max_acceleration * delta_t, max_vel );
}
jcmdh_.set_value( rcppmath::clamp( jcmdh_.get_value( ), min_vel, max_vel ) );
}
private:
double max_vel_limit_;
};
// TODO( anyone ): Port this to ROS 2
// //**
// * Interface for enforcing joint limits.
// *
// * \tparam HandleType %Handle type. Must implement the following methods:
// * \code
// * void enforce_limits( );
// * std::string get_name( ) const;
// * \endcode
// */
// template<class HandleType>
// class joint_limits_interface::JointLimitsInterface
// : public hardware_interface::ResourceManager<HandleType>
// {
// public:
// HandleType getHandle( const std::string & name )
// {
// // Rethrow exception with a meaningful type
// try {
// return this->hardware_interface::ResourceManager<HandleType>::getHandle( name );
// } catch ( const std::logic_error & e ) {
// throw joint_limits_interface::JointLimitsInterfaceException( e.what( ) );
// }
// }
//
// /** \name Real-Time Safe Functions
// *\{*/
// /** Enforce limits for all managed handles. */
// void enforce_limits( const rclcpp::Duration & period )
// {
// for ( auto && resource_name_and_handle : this->resource_map_ ) {
// resource_name_and_handle.second.enforce_limits( period );
// }
// }
// /*\}*/
// };
//
// /** Interface for enforcing limits on a position-controlled joint through saturation. */
// class PositionJointSaturationInterface
// : public joint_limits_interface::JointLimitsInterface<PositionJointSaturationHandle>
// {
// public:
// /** \name Real-Time Safe Functions
// *\{*/
// /** Reset all managed handles. */
// void reset( )
// {
// for ( auto && resource_name_and_handle : this->resource_map_ ) {
// resource_name_and_handle.second.reset( );
// }
// }
// /*\}*/
// };
//
// /** Interface for enforcing limits on a position-controlled joint with soft position limits. */
// class PositionJointSoftLimitsInterface
// : public joint_limits_interface::JointLimitsInterface<PositionJointSoftLimitsHandle>
// {
// public:
// /** \name Real-Time Safe Functions
// *\{*/
// /** Reset all managed handles. */
// void reset( )
// {
// for ( auto && resource_name_and_handle : this->resource_map_ ) {
// resource_name_and_handle.second.reset( );
// }
// }
// /*\}*/
// };
//
// /** Interface for enforcing limits on an effort-controlled joint through saturation. */
// class EffortJointSaturationInterface
// : public joint_limits_interface::JointLimitsInterface<EffortJointSaturationHandle>
// {
// };
//
// /** Interface for enforcing limits on an effort-controlled joint with soft position limits. */
// class EffortJointSoftLimitsInterface
// : public joint_limits_interface::JointLimitsInterface<EffortJointSoftLimitsHandle>
// {
// };
//
// /** Interface for enforcing limits on a velocity-controlled joint through saturation. */
// class VelocityJointSaturationInterface
// : public joint_limits_interface::JointLimitsInterface<VelocityJointSaturationHandle>
// {
// };
//
// /** Interface for enforcing limits on a velocity-controlled joint with soft position limits. */
// class VelocityJointSoftLimitsInterface
// : public joint_limits_interface::JointLimitsInterface<VelocityJointSoftLimitsHandle>
// {
// };
} // namespace joint_limits_interface
#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_HPP_
// Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_EXCEPTION_HPP_
#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_EXCEPTION_HPP_
#include <string>
namespace joint_limits_interface
{
/// An exception related to a \ref JointLimitsInterface
23 class JointLimitsInterfaceException : public std::exception
{
public:
26 explicit JointLimitsInterfaceException( const std::string & message ) : msg( message ) {}
const char * what( ) const noexcept override { return msg.c_str( ); }
private:
std::string msg;
};
} // namespace joint_limits_interface
#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_INTERFACE_EXCEPTION_HPP_
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/// \author Adolfo Rodriguez Tsouroukdissian
#ifndef JOINT_LIMITS_INTERFACE__JOINT_LIMITS_URDF_HPP_
#define JOINT_LIMITS_INTERFACE__JOINT_LIMITS_URDF_HPP_
#include <urdf/urdfdom_compatibility.h>
#include <urdf_model/joint.h>
#include <joint_limits_interface/joint_limits.hpp>
#include <rclcpp/rclcpp.hpp>
namespace joint_limits_interface
{
/**
* Populate a JointLimits instance from URDF joint data.
* \param[in] urdf_joint URDF joint.
* \param[out] limits Where URDF joint limit data gets written into. Limits in \e urdf_joint will overwrite existing
* values. Values in \e limits not present in \e urdf_joint remain unchanged.
* \return True if \e urdf_joint has a valid limits specification, false otherwise.
*/
34 inline bool getJointLimits( urdf::JointConstSharedPtr urdf_joint, JointLimits & limits )
{
if ( !urdf_joint || !urdf_joint->limits )
{
return false;
}
limits.has_position_limits =
urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::PRISMATIC;
if ( limits.has_position_limits )
{
limits.min_position = urdf_joint->limits->lower;
limits.max_position = urdf_joint->limits->upper;
}
if ( !limits.has_position_limits && urdf_joint->type == urdf::Joint::CONTINUOUS )
{
limits.angle_wraparound = true;
}
limits.has_velocity_limits = true;
limits.max_velocity = urdf_joint->limits->velocity;
limits.has_acceleration_limits = false;
limits.has_effort_limits = true;
limits.max_effort = urdf_joint->limits->effort;
return true;
}
/**
* Populate a SoftJointLimits instance from URDF joint data.
* \param[in] urdf_joint URDF joint.
* \param[out] soft_limits Where URDF soft joint limit data gets written into.
* \return True if \e urdf_joint has a valid soft limits specification, false otherwise.
*/
71 inline bool getSoftJointLimits( urdf::JointConstSharedPtr urdf_joint, SoftJointLimits & soft_limits )
{
if ( !urdf_joint || !urdf_joint->safety )
{
return false;
}
soft_limits.min_position = urdf_joint->safety->soft_lower_limit;
soft_limits.max_position = urdf_joint->safety->soft_upper_limit;
soft_limits.k_position = urdf_joint->safety->k_position;
soft_limits.k_velocity = urdf_joint->safety->k_velocity;
return true;
}
} // namespace joint_limits_interface
#endif // JOINT_LIMITS_INTERFACE__JOINT_LIMITS_URDF_HPP_
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/// \author Adolfo Rodriguez Tsouroukdissian
#include <gtest/gtest.h>
#include <joint_limits_interface/joint_limits_urdf.hpp>
#include <string>
23 class JointLimitsUrdfTest : public ::testing::Test
{
public:
26 JointLimitsUrdfTest( )
{
urdf_limits.reset( new urdf::JointLimits );
urdf_limits->effort = 8.0;
urdf_limits->velocity = 2.0;
urdf_limits->lower = -1.0;
urdf_limits->upper = 1.0;
urdf_safety.reset( new urdf::JointSafety );
urdf_safety->k_position = 20.0;
urdf_safety->k_velocity = 40.0;
urdf_safety->soft_lower_limit = -0.8;
urdf_safety->soft_upper_limit = 0.8;
urdf_joint.reset( new urdf::Joint );
urdf_joint->limits = urdf_limits;
urdf_joint->safety = urdf_safety;
urdf_joint->type = urdf::Joint::UNKNOWN;
}
protected:
48 urdf::JointLimitsSharedPtr urdf_limits;
49 urdf::JointSafetySharedPtr urdf_safety;
50 urdf::JointSharedPtr urdf_joint;
};
53 TEST_F( JointLimitsUrdfTest, GetJointLimits )
{
// Unset URDF joint
{
joint_limits_interface::JointLimits limits;
urdf::JointSharedPtr urdf_joint_bad;
EXPECT_FALSE( getJointLimits( urdf_joint_bad, limits ) );
}
// Unset URDF limits
{
joint_limits_interface::JointLimits limits;
urdf::JointSharedPtr urdf_joint_bad( new urdf::Joint );
EXPECT_FALSE( getJointLimits( urdf_joint_bad, limits ) );
}
// Valid URDF joint, CONTINUOUS type
{
urdf_joint->type = urdf::Joint::CONTINUOUS;
joint_limits_interface::JointLimits limits;
EXPECT_TRUE( getJointLimits( urdf_joint, limits ) );
// Position
EXPECT_FALSE( limits.has_position_limits );
EXPECT_TRUE( limits.angle_wraparound );
// Velocity
EXPECT_TRUE( limits.has_velocity_limits );
EXPECT_DOUBLE_EQ( urdf_joint->limits->velocity, limits.max_velocity );
// Acceleration
EXPECT_FALSE( limits.has_acceleration_limits );
// Effort
EXPECT_TRUE( limits.has_effort_limits );
EXPECT_DOUBLE_EQ( urdf_joint->limits->effort, limits.max_effort );
}
// Valid URDF joint, REVOLUTE type
{
urdf_joint->type = urdf::Joint::REVOLUTE;
joint_limits_interface::JointLimits limits;
EXPECT_TRUE( getJointLimits( urdf_joint, limits ) );
// Position
EXPECT_TRUE( limits.has_position_limits );
EXPECT_DOUBLE_EQ( urdf_joint->limits->lower, limits.min_position );
EXPECT_DOUBLE_EQ( urdf_joint->limits->upper, limits.max_position );
EXPECT_FALSE( limits.angle_wraparound );
// Velocity
EXPECT_TRUE( limits.has_velocity_limits );
EXPECT_DOUBLE_EQ( urdf_joint->limits->velocity, limits.max_velocity );
// Acceleration
EXPECT_FALSE( limits.has_acceleration_limits );
// Effort
EXPECT_TRUE( limits.has_effort_limits );
EXPECT_DOUBLE_EQ( urdf_joint->limits->effort, limits.max_effort );
}
// Valid URDF joint, PRISMATIC type
{
urdf_joint->type = urdf::Joint::PRISMATIC;
joint_limits_interface::JointLimits limits;
EXPECT_TRUE( getJointLimits( urdf_joint, limits ) );
// Position
EXPECT_TRUE( limits.has_position_limits );
EXPECT_DOUBLE_EQ( urdf_joint->limits->lower, limits.min_position );
EXPECT_DOUBLE_EQ( urdf_joint->limits->upper, limits.max_position );
EXPECT_FALSE( limits.angle_wraparound );
// Velocity
EXPECT_TRUE( limits.has_velocity_limits );
EXPECT_DOUBLE_EQ( urdf_joint->limits->velocity, limits.max_velocity );
// Acceleration
EXPECT_FALSE( limits.has_acceleration_limits );
// Effort
EXPECT_TRUE( limits.has_effort_limits );
EXPECT_DOUBLE_EQ( urdf_joint->limits->effort, limits.max_effort );
}
}
143 TEST_F( JointLimitsUrdfTest, GetSoftJointLimits )
{
// Unset URDF joint
{
joint_limits_interface::SoftJointLimits soft_limits;
urdf::JointSharedPtr urdf_joint_bad;
EXPECT_FALSE( getSoftJointLimits( urdf_joint_bad, soft_limits ) );
}
// Unset URDF limits
{
joint_limits_interface::SoftJointLimits soft_limits;
urdf::JointSharedPtr urdf_joint_bad( new urdf::Joint );
EXPECT_FALSE( getSoftJointLimits( urdf_joint_bad, soft_limits ) );
}
// Valid URDF joint
{
joint_limits_interface::SoftJointLimits soft_limits;
EXPECT_TRUE( getSoftJointLimits( urdf_joint, soft_limits ) );
// Soft limits
EXPECT_DOUBLE_EQ( urdf_joint->safety->soft_lower_limit, soft_limits.min_position );
EXPECT_DOUBLE_EQ( urdf_joint->safety->soft_upper_limit, soft_limits.max_position );
EXPECT_DOUBLE_EQ( urdf_joint->safety->k_position, soft_limits.k_position );
EXPECT_DOUBLE_EQ( urdf_joint->safety->k_velocity, soft_limits.k_velocity );
}
}
172 int main( int argc, char ** argv )
{
testing::InitGoogleTest( &argc, argv );
return RUN_ALL_TESTS( );
}
// Copyright ( c ) 2021 Stogl Robotics Consulting
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef ROS2_CONTROL_TEST_ASSETS__COMPONENTS_URDFS_HPP_
#define ROS2_CONTROL_TEST_ASSETS__COMPONENTS_URDFS_HPP_
#include <string>
namespace ros2_control_test_assets
{
// 1. Industrial Robots with only one interface
23 const auto valid_urdf_ros2_control_system_one_interface =
R"(
<ros2_control name="RRBotSystemPositionOnly" type="system">
<hardware>
<plugin>ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware</plugin>
<param name="example_param_write_for_sec">2</param>
<param name="example_param_read_for_sec">2</param>
</hardware>
<joint name="joint1">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
<joint name="joint2">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
</ros2_control>
)";
// 2. Industrial Robots with multiple interfaces ( cannot be written at the same time )
// Note, joint parameters can be any string
const auto valid_urdf_ros2_control_system_multi_interface =
R"(
<ros2_control name="RRBotSystemMultiInterface" type="system">
<hardware>
<plugin>ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware</plugin>
<param name="example_param_write_for_sec">2</param>
<param name="example_param_read_for_sec">2</param>
</hardware>
<joint name="joint1">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
<param name="initial_value">1.2</param>
</command_interface>
<command_interface name="velocity">
<param name="min">-1</param>
<param name="max">1</param>
<param name="initial_value">3.4</param>
</command_interface>
<command_interface name="effort">
<param name="min">-0.5</param>
<param name="max">0.5"</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="joint2">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
</ros2_control>
)";
// 3. Industrial Robots with integrated sensor
const auto valid_urdf_ros2_control_system_robot_with_sensor =
R"(
<ros2_control name="RRBotSystemWithSensor" type="system">
<hardware>
<plugin>ros2_control_demo_hardware/RRBotSystemWithSensorHardware</plugin>
<param name="example_param_write_for_sec">2</param>
<param name="example_param_read_for_sec">2</param>
</hardware>
<joint name="joint1">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
<joint name="joint2">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
<sensor name="tcp_fts_sensor">
<state_interface name="fx"/>
<state_interface name="fy"/>
<state_interface name="fz"/>
<state_interface name="tx"/>
<state_interface name="ty"/>
<state_interface name="tz"/>
<param name="frame_id">kuka_tcp</param>
<param name="lower_limits">-100</param>
<param name="upper_limits">100</param>
</sensor>
</ros2_control>
)";
// 4. Industrial Robots with externally connected sensor
const auto valid_urdf_ros2_control_system_robot_with_external_sensor =
R"(
<ros2_control name="RRBotSystemPositionOnlyWithExternalSensor" type="system">
<hardware>
<plugin>ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware</plugin>
<param name="example_param_write_for_sec">2</param>
<param name="example_param_read_for_sec">2</param>
</hardware>
<joint name="joint1">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
<joint name="joint2">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
</ros2_control>
<ros2_control name="RRBotForceTorqueSensor2D" type="sensor">
<hardware>
<plugin>ros2_control_demo_hardware/ForceTorqueSensor2DHardware</plugin>
<param name="example_param_read_for_sec">0.43</param>
</hardware>
<sensor name="tcp_fts_sensor">
<state_interface name="fx"/>
<state_interface name="fy"/>
<state_interface name="fz"/>
<state_interface name="tx"/>
<state_interface name="ty"/>
<state_interface name="tz"/>
<param name="frame_id">kuka_tcp</param>
<param name="lower_limits">-100</param>
<param name="upper_limits">100</param>
</sensor>
</ros2_control>
)";
// 5. Modular Robots with separate communication to each actuator
const auto valid_urdf_ros2_control_actuator_modular_robot =
R"(
<ros2_control name="RRBotModularJoint1" type="actuator">
<hardware>
<plugin>ros2_control_demo_hardware/PositionActuatorHardware</plugin>
<param name="example_param_write_for_sec">1.23</param>
<param name="example_param_read_for_sec">3</param>
</hardware>
<joint name="joint1">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
</ros2_control>
<ros2_control name="RRBotModularJoint2" type="actuator">
<hardware>
<plugin>ros2_control_demo_hardware/PositionActuatorHardware</plugin>
<param name="example_param_write_for_sec">1.23</param>
<param name="example_param_read_for_sec">3</param>
</hardware>
<joint name="joint2">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
</ros2_control>
)";
// 6. Modular Robots with actuators not providing states and with additional sensors
// Example for simple transmission
const auto valid_urdf_ros2_control_actuator_modular_robot_sensors =
R"(
<ros2_control name="RRBotModularJoint1" type="actuator">
<hardware>
<plugin>ros2_control_demo_hardware/VelocityActuatorHardware</plugin>
<param name="example_param_write_for_sec">1.23</param>
<param name="example_param_read_for_sec">3</param>
</hardware>
<joint name="joint1">
<command_interface name="velocity">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
<transmission name="transmission1">
<plugin>transmission_interface/SimpleTansmission</plugin>
<joint name="joint1" role="joint1">
<mechanical_reduction>325.949</mechanical_reduction>
</joint>
</transmission>
</ros2_control>
<ros2_control name="RRBotModularJoint2" type="actuator">
<hardware>
<plugin>ros2_control_demo_hardware/VelocityActuatorHardware</plugin>
<param name="example_param_write_for_sec">1.23</param>
<param name="example_param_read_for_sec">3</param>
</hardware>
<joint name="joint2">
<command_interface name="velocity">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
</ros2_control>
<ros2_control name="RRBotModularPositionSensorJoint1" type="sensor">
<hardware>
<plugin>ros2_control_demo_hardware/PositionSensorHardware</plugin>
<param name="example_param_read_for_sec">2</param>
</hardware>
<joint name="joint1">
<state_interface name="position"/>
</joint>
</ros2_control>
<ros2_control name="RRBotModularPositionSensorJoint2" type="sensor">
<hardware>
<plugin>ros2_control_demo_hardware/PositionSensorHardware</plugin>
<param name="example_param_read_for_sec">2</param>
</hardware>
<joint name="joint2">
<state_interface name="position"/>
</joint>
</ros2_control>
)";
// 7. Modular Robots with separate communication to each "actuator" with multi joints
// Example for complex transmission ( multi-joint/multi-actuator ) - ( system component is used )
const auto valid_urdf_ros2_control_system_multi_joints_transmission =
R"(
<ros2_control name="RRBotModularWrist" type="system">
<hardware>
<plugin>ros2_control_demo_hardware/ActuatorHardwareMultiDOF</plugin>
<param name="example_param_write_for_sec">1.23</param>
<param name="example_param_read_for_sec">3</param>
</hardware>
<joint name="joint1">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
<joint name="joint2">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
<transmission name="transmission1">
<plugin>transmission_interface/DifferentialTransmission</plugin>
<actuator name="joint1_motor" role="actuator1"/>
<actuator name="joint2_motor" role="actuator2"/>
<joint name="joint1" role="joint1">
<mechanical_reduction>10</mechanical_reduction>
<offset>0.5</offset>
</joint>
<joint name="joint2" role="joint2">
<mechanical_reduction>50</mechanical_reduction>
</joint>
</transmission>
</ros2_control>
)";
// 8. Sensor only
const auto valid_urdf_ros2_control_sensor_only =
R"(
<ros2_control name="CameraWithIMU" type="sensor">
<hardware>
<plugin>ros2_control_demo_hardware/CameraWithIMUSensor</plugin>
<param name="example_param_read_for_sec">2</param>
</hardware>
<sensor name="sensor1">
<state_interface name="roll"/>
<state_interface name="pitch"/>
<state_interface name="yaw"/>
</sensor>
<sensor name="sensor2">
<state_interface name="image"/>
</sensor>
</ros2_control>
)";
// 9. Actuator Only
const auto valid_urdf_ros2_control_actuator_only =
R"(
<ros2_control name="ActuatorModularJoint1" type="actuator">
<hardware>
<plugin>ros2_control_demo_hardware/VelocityActuatorHardware</plugin>
<param name="example_param_write_for_sec">1.13</param>
<param name="example_param_read_for_sec">3</param>
</hardware>
<joint name="joint1">
<command_interface name="velocity">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
<transmission name="transmission1">
<plugin>transmission_interface/RotationToLinerTansmission</plugin>
<joint name="joint1" role="joint1">
<mechanical_reduction>325.949</mechanical_reduction>
</joint>
<param name="additional_special_parameter">1337</param>
</transmission>
</ros2_control>
)";
// 10. Industrial Robots with integrated GPIO
const auto valid_urdf_ros2_control_system_robot_with_gpio =
R"(
<ros2_control name="RRBotSystemWithGPIO" type="system">
<hardware>
<plugin>ros2_control_demo_hardware/RRBotSystemWithGPIOHardware</plugin>
<param name="example_param_write_for_sec">2</param>
<param name="example_param_read_for_sec">2</param>
</hardware>
<joint name="joint1">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
<joint name="joint2">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
<gpio name="flange_analog_IOs">
<command_interface name="analog_output1"/>
<state_interface name="analog_output1"/> <!-- Needed to know current state of the output -->
<state_interface name="analog_input1"/>
<state_interface name="analog_input2"/>
</gpio>
<gpio name="flange_vacuum">
<command_interface name="vacuum"/>
<state_interface name="vacuum"/> <!-- Needed to know current state of the input -->
</gpio>
</ros2_control>
)";
// 11. Industrial Robots using size and data_type attributes
const auto valid_urdf_ros2_control_system_robot_with_size_and_data_type =
R"(
<ros2_control name="RRBotSystemWithSizeAndDataType" type="system">
<hardware>
<plugin>ros2_control_demo_hardware/RRBotSystemWithSizeAndDataType</plugin>
<param name="example_param_write_for_sec">2</param>
<param name="example_param_read_for_sec">2</param>
</hardware>
<joint name="joint1">
<command_interface name="position"/>
<state_interface name="position"/>
</joint>
<gpio name="flange_IOS">
<command_interface name="digital_output" size="2" data_type="bool"/>
<state_interface name="analog_input" size="3"/>
<state_interface name="image" data_type="cv::Mat"/>
</gpio>
</ros2_control>
)";
// Errors
const auto invalid_urdf_ros2_control_invalid_child =
R"(
<ros2_control name="2DOF_System_Robot_Position_Only" type="system">
<hardwarert>
<plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
<param name="example_param_write_for_sec">2</param>
<param name="example_param_read_for_sec">2</param>
</hardwarert>
</ros2_control>
)";
const auto invalid_urdf_ros2_control_missing_attribute =
R"(
<ros2_control type="system">
<hardware>
<plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
<param name="example_param_write_for_sec">2</param>
<param name="example_param_read_for_sec">2</param>
</hardware>
</ros2_control>
)";
const auto invalid_urdf_ros2_control_component_missing_class_type =
R"(
<ros2_control name="2DOF_System_Robot_Position_Only" type="system">
<hardware>
<plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
<param name="example_param_write_for_sec">2</param>
<param name="example_param_read_for_sec">2</param>
</hardware>
<joint name="joint1">
<param name="min_position_value">-1</param>
<param name="max_position_value">1</param>
</joint>
</ros2_control>
)";
const auto invalid_urdf_ros2_control_parameter_missing_name =
R"(
<ros2_control name="2DOF_System_Robot_Position_Only" type="system">
<hardware>
<plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
<param>2</param>
<param name="example_param_read_for_sec">2</param>
</hardware>
<joint name="joint1">
<plugin>ros2_control_components/PositionJoint</plugin>
<param name="min_position_value">-1</param>
<param name="max_position_value">1</param>
</joint>
</ros2_control>
)";
const auto invalid_urdf_ros2_control_component_class_type_empty =
R"(
<ros2_control name="2DOF_System_Robot_Position_Only" type="system">
<hardware>
<plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
<param name="example_param_write_for_sec">2</param>
<param name="example_param_read_for_sec">2</param>
</hardware>
<joint name="joint1">
<plugin></plugin>
<param name="min_position_value">-1</param>
<param name="max_position_value">1</param>
</joint>
</ros2_control>
)";
const auto invalid_urdf_ros2_control_component_interface_type_empty =
R"(
<ros2_control name="2DOF_System_Robot_Position_Only" type="system">
<hardware>
<plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
<param name="example_param_write_for_sec">2</param>
<param name="example_param_read_for_sec">2</param>
</hardware>
<joint name="joint1">
<plugin>ros2_control_components/PositionJoint</plugin>
<state_interface></state_interface>
<param name="min_position_value">-1</param>
<param name="max_position_value">1</param>
</joint>
</ros2_control>
)";
const auto invalid_urdf_ros2_control_parameter_empty =
R"(
<ros2_control name="2DOF_System_Robot_Position_Only" type="system">
<hardware>
<plugin>ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only</plugin>
<param name="example_param_write_for_sec"></param>
<param name="example_param_read_for_sec">2</param>
</hardware>
<joint name="joint1">
<command_interface name="position">
<param name="min_position_value">-1</param>
<param name="max_position_value">1</param>
</command_interface>
</joint>
</ros2_control>
)";
const auto invalid_urdf2_ros2_control_illegal_size =
R"(
<ros2_control name="RRBotSystemWithIllegalSize" type="system">
<hardware>
<plugin>ros2_control_demo_hardware/RRBotSystemWithIllegalSize</plugin>
</hardware>
<gpio name="flange_IOS">
<command_interface name="digital_output" data_type="bool" size="-4"/>
</gpio>
</ros2_control>
)";
const auto invalid_urdf2_ros2_control_illegal_size2 =
R"(
<ros2_control name="RRBotSystemWithIllegalSize2" type="system">
<hardware>
<plugin>ros2_control_demo_hardware/RRBotSystemWithIllegalSize2</plugin>
</hardware>
<gpio name="flange_IOS">
<command_interface name="digital_output" data_type="bool" size="ILLEGAL"/>
</gpio>
</ros2_control>
)";
const auto invalid_urdf2_hw_transmission_joint_mismatch =
R"(
<ros2_control name="ActuatorModularJoint1" type="actuator">
<hardware>
<plugin>ros2_control_demo_hardware/VelocityActuatorHardware</plugin>
</hardware>
<joint name="joint1">
<command_interface name="velocity">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
<transmission name="transmission1">
<plugin>transmission_interface/SimpleTransmission</plugin>
<joint name="joint31415" role="joint1"/>
</transmission>
</ros2_control>
)";
const auto invalid_urdf2_transmission_given_too_many_joints =
R"(
<ros2_control name="ActuatorModularJoint1" type="actuator">
<hardware>
<plugin>ros2_control_demo_hardware/VelocityActuatorHardware</plugin>
</hardware>
<joint name="joint1">
<command_interface name="velocity">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
<transmission name="transmission1">
<plugin>transmission_interface/SimpleTransmission</plugin>
<joint name="joint1" role="joint1"/>
<joint name="joint2" role="joint2"/>
</transmission>
</ros2_control>
)";
} // namespace ros2_control_test_assets
#endif // ROS2_CONTROL_TEST_ASSETS__COMPONENTS_URDFS_HPP_
// Copyright 2020 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef ROS2_CONTROL_TEST_ASSETS__DESCRIPTIONS_HPP_
#define ROS2_CONTROL_TEST_ASSETS__DESCRIPTIONS_HPP_
#include <string>
#include <vector>
namespace ros2_control_test_assets
{
23 const auto urdf_head =
R"(
<?xml version="1.0" encoding="utf-8"?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from minimal_robot.urdf.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="MinimalRobot">
<!-- Used for fixing robot -->
<link name="world"/>
<joint name="base_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="world"/>
<child link="base_link"/>
</joint>
<link name="base_link">
<inertial>
<mass value="0.01"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.2" radius="0.1"/>
</geometry>
<material name="DarkGrey">
<color rgba="0.4 0.4 0.4 1.0"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="1" radius="0.1"/>
</geometry>
</collision>
</link>
<joint name="joint1" type="revolute">
<origin rpy="-1.57079632679 0 0" xyz="0 0 0.2"/>
<parent link="base_link"/>
<child link="link1"/>
<limit effort="0.1" lower="-3.14159265359" upper="3.14159265359" velocity="0.2"/>
</joint>
<link name="link1">
<inertial>
<mass value="0.01"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="1" radius="0.1"/>
</geometry>
<material name="DarkGrey">
<color rgba="0.4 0.4 0.4 1.0"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="1" radius="0.1"/>
</geometry>
</collision>
</link>
<joint name="joint2" type="revolute">
<origin rpy="1.57079632679 0 0" xyz="0 0 0.9"/>
<parent link="link1"/>
<child link="link2"/>
<limit effort="0.1" lower="-3.14159265359" upper="3.14159265359" velocity="0.2"/>
</joint>
<link name="link2">
<inertial>
<mass value="0.01"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="1" radius="0.1"/>
</geometry>
<material name="DarkGrey">
<color rgba="0.4 0.4 0.4 1.0"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="1" radius="0.1"/>
</geometry>
</collision>
</link>
<joint name="tool_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 1"/>
<parent link="link2"/>
<child link="tool_link"/>
</joint>
<link name="tool_link">
</link>
)";
const auto urdf_tail =
R"(
</robot>
)";
const auto hardware_resources =
R"(
<ros2_control name="TestActuatorHardware" type="actuator">
<hardware>
<plugin>test_actuator</plugin>
</hardware>
<joint name="joint1">
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<command_interface name="max_velocity" />
</joint>
</ros2_control>
<ros2_control name="TestSensorHardware" type="sensor">
<hardware>
<plugin>test_sensor</plugin>
<param name="example_param_write_for_sec">2</param>
<param name="example_param_read_for_sec">2</param>
</hardware>
<sensor name="sensor1">
<state_interface name="velocity"/>
</sensor>
</ros2_control>
<ros2_control name="TestSystemHardware" type="system">
<hardware>
<plugin>test_system</plugin>
<param name="example_param_write_for_sec">2</param>
<param name="example_param_read_for_sec">2</param>
</hardware>
<joint name="joint2">
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="acceleration"/>
<command_interface name="max_acceleration" />
</joint>
<joint name="joint3">
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="acceleration"/>
</joint>
<joint name="configuration">
<command_interface name="max_tcp_jerk"/>
<state_interface name="max_tcp_jerk"/>
</joint>
</ros2_control>
)";
const auto hardware_resources_missing_state_keys =
R"(
<ros2_control name="TestActuatorHardware" type="actuator">
<hardware>
<plugin>test_actuator</plugin>
</hardware>
<joint name="joint1">
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
</ros2_control>
<ros2_control name="TestSensorHardware" type="sensor">
<hardware>
<plugin>test_sensor</plugin>
<param name="example_param_write_for_sec">2</param>
<param name="example_param_read_for_sec">2</param>
</hardware>
<sensor name="sensor1">
<state_interface name="velocity"/>
<state_interface name="does_not_exist"/>
</sensor>
</ros2_control>
<ros2_control name="TestSystemHardware" type="system">
<hardware>
<plugin>test_system</plugin>
<param name="example_param_write_for_sec">2</param>
<param name="example_param_read_for_sec">2</param>
</hardware>
<joint name="joint2">
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="does_not_exist"/>
</joint>
<joint name="joint3">
<command_interface name="velocity"/>
<state_interface name="position"/>
<state_interface name="does_not_exist"/>
</joint>
</ros2_control>
)";
const auto hardware_resources_missing_command_keys =
R"(
<ros2_control name="TestActuatorHardware" type="actuator">
<hardware>
<plugin>test_actuator</plugin>
</hardware>
<joint name="joint1">
<command_interface name="position"/>
<command_interface name="does_not_exist"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
</ros2_control>
<ros2_control name="TestSensorHardware" type="sensor">
<hardware>
<plugin>test_sensor</plugin>
<param name="example_param_write_for_sec">2</param>
<param name="example_param_read_for_sec">2</param>
</hardware>
<sensor name="sensor1">
<state_interface name="velocity"/>
</sensor>
</ros2_control>
<ros2_control name="TestSystemHardware" type="system">
<hardware>
<plugin>test_system</plugin>
<param name="example_param_write_for_sec">2</param>
<param name="example_param_read_for_sec">2</param>
</hardware>
<joint name="joint2">
<command_interface name="velocity"/>
<command_interface name="does_not_exist"/>
<state_interface name="position"/>
</joint>
<joint name="joint3">
<command_interface name="velocity"/>
<command_interface name="does_not_exist"/>
<state_interface name="position"/>
</joint>
</ros2_control>
)";
const auto diffbot_urdf =
R"(
<?xml version="1.0" ?>
<robot name="robot">
<!-- Space btw top of beam and the each joint -->
<!-- Links: inertial, visual, collision -->
<link name="base_link">
<inertial>
<!-- origin is relative -->
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="5"/>
<inertia ixx="5" ixy="0" ixz="0" iyy="5" iyz="0" izz="5"/>
</inertial>
<visual>
<geometry>
<box size="0.5 0.1 0.1"/>
</geometry>
</visual>
<collision>
<!-- origin is relative -->
<origin xyz="0 0 0"/>
<geometry>
<box size="0.5 0.1 0.1"/>
</geometry>
</collision>
</link>
<link name="base_footprint">
<visual>
<geometry>
<sphere radius="0.01"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0"/>
<geometry>
<sphere radius="0.00000001"/>
</geometry>
</collision>
</link>
<joint name="base_footprint_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0.11"/>
<child link="base_link"/>
<parent link="base_footprint"/>
</joint>
<link name="wheel_0_link">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="1"/>
<inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.086" radius="0.11"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.086" radius="0.11"/>
</geometry>
</collision>
</link>
<joint name="wheel_0_joint" type="continuous">
<parent link="base_link"/>
<child link="wheel_0_link"/>
<origin rpy="-1.5707963267948966 0 0" xyz="0.3 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<!-- Transmission is important to link the joints and the controller -->
<transmission name="wheel_0_joint_trans" type="SimpleTransmission">
<actuator name="wheel_0_joint_motor"/>
<joint name="wheel_0_joint"/>
<mechanicalReduction>1</mechanicalReduction>
<motorTorqueConstant>1</motorTorqueConstant>
</transmission>
<gazebo reference="wheel_0_link">
<material>Gazebo/Red</material>
</gazebo>
<link name="wheel_1_link">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="1"/>
<inertia ixx="0.2" ixy="0" ixz="0" iyy="0.2" iyz="0" izz="0.2"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.086" radius="0.11"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.086" radius="0.11"/>
</geometry>
</collision>
</link>
<joint name="wheel_1_joint" type="continuous">
<parent link="base_link"/>
<child link="wheel_1_link"/>
<origin rpy="-1.5707963267948966 0 0" xyz="-0.2 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<!-- Transmission is important to link the joints and the controller -->
<transmission name="wheel_1_joint_trans" type="SimpleTransmission">
<actuator name="wheel_1_joint_motor"/>
<joint name="wheel_1_joint"/>
<mechanicalReduction>1</mechanicalReduction>
<motorTorqueConstant>1</motorTorqueConstant>
</transmission>
<gazebo reference="wheel_1_link">
<material>Gazebo/Red</material>
</gazebo>
<!-- Colour -->
<gazebo reference="base_link">
<material>Gazebo/Green</material>
</gazebo>
<gazebo reference="base_footprint">
<material>Gazebo/Purple</material>
</gazebo>
<ros2_control name="TestActuatorHardwareLeft" type="actuator">
<hardware>
<plugin>test_actuator</plugin>
</hardware>
<joint name="wheel_left">
<state_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="velocity"/>
</joint>
</ros2_control>
<ros2_control name="TestActuatorHardwareRight" type="actuator">
<hardware>
<plugin>test_actuator</plugin>
</hardware>
<joint name="wheel_right">
<state_interface name="position"/>
<command_interface name="velocity"/>
<state_interface name="velocity"/>
</joint>
</ros2_control>
</robot>
)";
const auto minimal_robot_urdf =
std::string( urdf_head ) + std::string( hardware_resources ) + std::string( urdf_tail );
const auto minimal_robot_missing_state_keys_urdf =
std::string( urdf_head ) + std::string( hardware_resources_missing_state_keys ) +
std::string( urdf_tail );
const auto minimal_robot_missing_command_keys_urdf =
std::string( urdf_head ) + std::string( hardware_resources_missing_command_keys ) +
std::string( urdf_tail );
[[maybe_unused]] const std::string TEST_ACTUATOR_HARDWARE_NAME = "TestActuatorHardware";
[[maybe_unused]] const std::string TEST_ACTUATOR_HARDWARE_TYPE = "actuator";
[[maybe_unused]] const std::string TEST_ACTUATOR_HARDWARE_CLASS_TYPE = "test_actuator";
[[maybe_unused]] const std::vector<std::string> TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES = {
"joint1/position", "joint1/max_velocity"};
[[maybe_unused]] const std::vector<std::string> TEST_ACTUATOR_HARDWARE_STATE_INTERFACES = {
"joint1/position", "joint1/velocity", "joint1/some_unlisted_interface"};
[[maybe_unused]] const std::string TEST_SENSOR_HARDWARE_NAME = "TestSensorHardware";
[[maybe_unused]] const std::string TEST_SENSOR_HARDWARE_TYPE = "sensor";
[[maybe_unused]] const std::string TEST_SENSOR_HARDWARE_CLASS_TYPE = "test_sensor";
[[maybe_unused]] const std::vector<std::string> TEST_SENSOR_HARDWARE_COMMAND_INTERFACES = {""};
[[maybe_unused]] const std::vector<std::string> TEST_SENSOR_HARDWARE_STATE_INTERFACES = {
"sensor1/velocity"};
[[maybe_unused]] const std::string TEST_SYSTEM_HARDWARE_NAME = "TestSystemHardware";
[[maybe_unused]] const std::string TEST_SYSTEM_HARDWARE_TYPE = "system";
[[maybe_unused]] const std::string TEST_SYSTEM_HARDWARE_CLASS_TYPE = "test_system";
[[maybe_unused]] const std::vector<std::string> TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES = {
"joint2/velocity", "joint2/max_acceleration", "joint3/velocity", "configuration/max_tcp_jerk"};
[[maybe_unused]] const std::vector<std::string> TEST_SYSTEM_HARDWARE_STATE_INTERFACES = {
"joint2/position", "joint2/velocity", "joint2/acceleration", "joint3/position",
"joint3/velocity", "joint3/acceleration", "configuration/max_tcp_jerk"};
} // namespace ros2_control_test_assets
#endif // ROS2_CONTROL_TEST_ASSETS__DESCRIPTIONS_HPP_
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TRANSMISSION_INTERFACE__ACCESSOR_HPP_
#define TRANSMISSION_INTERFACE__ACCESSOR_HPP_
#include <algorithm>
#include <set>
#include <string>
#include <vector>
namespace transmission_interface
{
template <typename T>
26 std::string to_string( const std::vector<T> & list )
{
std::stringstream ss;
ss << "[";
for ( const auto & elem : list )
{
ss << elem << ", ";
}
if ( !list.empty( ) )
{
ss.seekp( -2, std::ios_base::end ); // remove last ", "
}
ss << "]";
return ss.str( );
}
template <class T>
44 std::vector<std::string> get_names( const std::vector<T> & handles )
{
std::set<std::string> names;
std::transform(
handles.cbegin( ), handles.cend( ), std::inserter( names, names.end( ) ),
[]( const auto & handle ) { return handle.get_name( ); } );
return std::vector<std::string>( names.begin( ), names.end( ) );
}
template <typename T>
54 std::vector<T> get_ordered_handles(
const std::vector<T> & unordered_handles, const std::vector<std::string> & names,
const std::string & interface_type )
{
std::vector<T> result;
for ( const auto & name : names )
{
std::copy_if(
unordered_handles.cbegin( ), unordered_handles.cend( ), std::back_inserter( result ),
[&]( const auto & handle )
{
return ( handle.get_name( ) == name ) && ( handle.get_interface_name( ) == interface_type ) &&
handle;
} );
}
return result;
}
} // namespace transmission_interface
#endif // TRANSMISSION_INTERFACE__ACCESSOR_HPP_
// Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TRANSMISSION_INTERFACE__DIFFERENTIAL_TRANSMISSION_HPP_
#define TRANSMISSION_INTERFACE__DIFFERENTIAL_TRANSMISSION_HPP_
#include <cassert>
#include <set>
#include <string>
#include <vector>
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "transmission_interface/accessor.hpp"
#include "transmission_interface/exception.hpp"
#include "transmission_interface/transmission.hpp"
namespace transmission_interface
{
/// Implementation of a differential transmission.
/**
*
* This transmission relates <b>two actuators</b> and <b>two joints</b> through a differential mechanism, as illustrated
* below.
* \image html differential_transmission.png
*
* <CENTER>
* <table>
* <tr><th></th><th><CENTER>Effort</CENTER></th><th><CENTER>Velocity</CENTER></th><th><CENTER>Position</CENTER></th></tr>
* <tr><td>
* <b> Actuator to joint </b>
* </td>
* <td>
* \f{eqnarray*}{
* \tau_{j_1} & = & n_{j_1} ( n_{a_1} \tau_{a_1} + n_{a_2} \tau_{a_2} ) \\[2.5em]
* \tau_{j_2} & = & n_{j_2} ( n_{a_1} \tau_{a_1} + n_{a_2} \tau_{a_2} )
* \f}
* </td>
* <td>
* \f{eqnarray*}{
* \dot{x}_{j_1} & = & \frac{ \dot{x}_{a_1} / n_{a_1} + \dot{x}_{a_2} / n_{a_2} }{2 n_{j_1}} \\[1em]
* \dot{x}_{j_2} & = & \frac{ \dot{x}_{a_1} / n_{a_1} - \dot{x}_{a_2} / n_{a_2} }{2 n_{j_2}}
* \f}
* </td>
* <td>
* \f{eqnarray*}{
* x_{j_1} & = & \frac{ x_{a_1} / n_{a_1} + x_{a_2} / n_{a_2} }{2 n_{j_1}} + x_{off_1} \\[1em]
* x_{j_2} & = & \frac{ x_{a_1} / n_{a_1} - x_{a_2} / n_{a_2} }{2 n_{j_1}} + x_{off_2}
* \f}
* </td>
* </tr>
* <tr><td>
* <b> Joint to actuator </b>
* </td>
* <td>
* \f{eqnarray*}{
* \tau_{a_1} & = & \frac{ \tau_{j_1} / n_{j_1} + \tau_{j_2} / n_{j_2} }{2 n_{a_1}} \\[1em]
* \tau_{a_2} & = & \frac{ \tau_{j_1} / n_{j_1} - \tau_{j_2} / n_{j_2} }{2 n_{a_1}}
* \f}
* </td>
* <td>
* \f{eqnarray*}{
* \dot{x}_{a_1} & = & n_{a_1} ( n_{j_1} \dot{x}_{j_1} + n_{j_2} \dot{x}_{j_2} ) \\[2.5em]
* \dot{x}_{a_2} & = & n_{a_2} ( n_{j_1} \dot{x}_{j_1} - n_{j_2} \dot{x}_{j_2} )
* \f}
* </td>
* <td>
* \f{eqnarray*}{
* x_{a_1} & = & n_{a_1} \left[ n_{j_1} ( x_{j_1} - x_{off_1} ) + n_{j_2} ( x_{j_2} - x_{off_2} ) \right] \\[2.5em]
* x_{a_2} & = & n_{a_2} \left[ n_{j_1} ( x_{j_1} - x_{off_1} ) - n_{j_2} ( x_{j_2} - x_{off_2} ) \right]
* \f}
* </td></tr></table>
* </CENTER>
*
* where:
* - \f$ x \f$, \f$ \dot{x} \f$ and \f$ \tau \f$ are position, velocity and effort variables, respectively.
* - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space variables, respectively.
* - \f$ x_{off}\f$ represents the offset between motor and joint zeros, expressed in joint position coordinates
* ( cf. SimpleTransmission class documentation for a more detailed description of this variable ).
* - \f$ n \f$ represents a transmission ratio. Reducers/amplifiers are allowed on both the actuator and joint sides
* ( depicted as timing belts in the figure ).
* A transmission ratio can take any real value \e except zero. In particular:
* - If its absolute value is greater than one, it's a velocity reducer / effort amplifier, while if its absolute
* value lies in \f$ ( 0, 1 ) \f$ it's a velocity amplifier / effort reducer.
* - Negative values represent a direction flip, ie. input and output move in opposite directions.
* - <b>Important:</b> Use transmission ratio signs to match this class' convention of positive actuator/joint
* directions with a given mechanical design, as they will in general not match.
*
* \note This implementation currently assumes a specific layout for location of the actuators and joint axes which is
* common in robotic mechanisms. Please file an enhancement ticket if your use case does not adhere to this layout.
*
* \ingroup transmission_types
*/
104 class DifferentialTransmission : public Transmission
{
public:
/**
* \param[in] actuator_reduction Reduction ratio of actuators.
* \param[in] joint_reduction Reduction ratio of joints.
* \param[in] joint_offset Joint position offset used in the position mappings.
* \pre Nonzero actuator and joint reduction values.
*/
113 DifferentialTransmission(
const std::vector<double> & actuator_reduction, const std::vector<double> & joint_reduction,
const std::vector<double> & joint_offset = {0.0, 0.0} );
/**
* \param[in] joint_handles Handles of joint values.
* \param[in] actuator_handles Handles of actuator values.
* \pre Handles are valid and matching in size
*/
void configure(
const std::vector<JointHandle> & joint_handles,
const std::vector<ActuatorHandle> & actuator_handles ) override;
/// Transform variables from actuator to joint space.
/**
* \pre Actuator and joint vectors must have size 2 and point to valid data.
* To call this method it is not required that all other data vectors contain valid data, and can even remain empty.
*/
void actuator_to_joint( ) override;
/// Transform variables from joint to actuator space.
/**
* \pre Actuator and joint vectors must have size 2 and point to valid data.
* To call this method it is not required that all other data vectors contain valid data, and can even remain empty.
*/
void joint_to_actuator( ) override;
std::size_t num_actuators( ) const override { return 2; }
141 std::size_t num_joints( ) const override { return 2; }
143 const std::vector<double> & get_actuator_reduction( ) const { return actuator_reduction_; }
144 const std::vector<double> & get_joint_reduction( ) const { return joint_reduction_; }
145 const std::vector<double> & get_joint_offset( ) const { return joint_offset_; }
/// Get human-friendly report of handles
148 std::string get_handles_info( ) const;
protected:
std::vector<double> actuator_reduction_;
std::vector<double> joint_reduction_;
std::vector<double> joint_offset_;
std::vector<JointHandle> joint_position_;
std::vector<JointHandle> joint_velocity_;
std::vector<JointHandle> joint_effort_;
std::vector<ActuatorHandle> actuator_position_;
std::vector<ActuatorHandle> actuator_velocity_;
std::vector<ActuatorHandle> actuator_effort_;
};
inline DifferentialTransmission::DifferentialTransmission(
const std::vector<double> & actuator_reduction, const std::vector<double> & joint_reduction,
const std::vector<double> & joint_offset )
: actuator_reduction_( actuator_reduction ),
joint_reduction_( joint_reduction ),
joint_offset_( joint_offset )
{
if (
num_actuators( ) != actuator_reduction_.size( ) || num_joints( ) != joint_reduction_.size( ) ||
num_joints( ) != joint_offset_.size( ) )
{
throw Exception( "Reduction and offset vectors must have size 2." );
}
if (
0.0 == actuator_reduction_[0] || 0.0 == actuator_reduction_[1] || 0.0 == joint_reduction_[0] ||
0.0 == joint_reduction_[1] )
{
throw Exception( "Transmission reduction ratios cannot be zero." );
}
}
void DifferentialTransmission::configure(
const std::vector<JointHandle> & joint_handles,
const std::vector<ActuatorHandle> & actuator_handles )
{
if ( joint_handles.empty( ) )
{
throw Exception( "No joint handles were passed in" );
}
if ( actuator_handles.empty( ) )
{
throw Exception( "No actuator handles were passed in" );
}
const auto joint_names = get_names( joint_handles );
if ( joint_names.size( ) != 2 )
{
throw Exception(
"There should be exactly two unique joint names but was given " + to_string( joint_names ) );
}
const auto actuator_names = get_names( actuator_handles );
if ( actuator_names.size( ) != 2 )
{
throw Exception(
"There should be exactly two unique actuator names but was given " +
to_string( actuator_names ) );
}
joint_position_ =
get_ordered_handles( joint_handles, joint_names, hardware_interface::HW_IF_POSITION );
joint_velocity_ =
get_ordered_handles( joint_handles, joint_names, hardware_interface::HW_IF_VELOCITY );
joint_effort_ = get_ordered_handles( joint_handles, joint_names, hardware_interface::HW_IF_EFFORT );
if ( joint_position_.size( ) != 2 && joint_velocity_.size( ) != 2 && joint_effort_.size( ) != 2 )
{
throw Exception( "Not enough valid or required joint handles were presented." );
}
actuator_position_ =
get_ordered_handles( actuator_handles, actuator_names, hardware_interface::HW_IF_POSITION );
actuator_velocity_ =
get_ordered_handles( actuator_handles, actuator_names, hardware_interface::HW_IF_VELOCITY );
actuator_effort_ =
get_ordered_handles( actuator_handles, actuator_names, hardware_interface::HW_IF_EFFORT );
if (
actuator_position_.size( ) != 2 && actuator_velocity_.size( ) != 2 &&
actuator_effort_.size( ) != 2 )
{
throw Exception(
"Not enough valid or required actuator handles were presented. \n" + get_handles_info( ) );
}
if (
joint_position_.size( ) != actuator_position_.size( ) &&
joint_velocity_.size( ) != actuator_velocity_.size( ) &&
joint_effort_.size( ) != actuator_effort_.size( ) )
{
throw Exception( "Pair-wise mismatch on interfaces. \n" + get_handles_info( ) );
}
}
inline void DifferentialTransmission::actuator_to_joint( )
{
const auto & ar = actuator_reduction_;
const auto & jr = joint_reduction_;
auto & act_pos = actuator_position_;
auto & joint_pos = joint_position_;
if ( act_pos.size( ) == num_actuators( ) && joint_pos.size( ) == num_joints( ) )
{
assert( act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1] );
joint_pos[0].set_value(
( act_pos[0].get_value( ) / ar[0] + act_pos[1].get_value( ) / ar[1] ) / ( 2.0 * jr[0] ) +
joint_offset_[0] );
joint_pos[1].set_value(
( act_pos[0].get_value( ) / ar[0] - act_pos[1].get_value( ) / ar[1] ) / ( 2.0 * jr[1] ) +
joint_offset_[1] );
}
auto & act_vel = actuator_velocity_;
auto & joint_vel = joint_velocity_;
if ( act_vel.size( ) == num_actuators( ) && joint_vel.size( ) == num_joints( ) )
{
assert( act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1] );
joint_vel[0].set_value(
( act_vel[0].get_value( ) / ar[0] + act_vel[1].get_value( ) / ar[1] ) / ( 2.0 * jr[0] ) );
joint_vel[1].set_value(
( act_vel[0].get_value( ) / ar[0] - act_vel[1].get_value( ) / ar[1] ) / ( 2.0 * jr[1] ) );
}
auto & act_eff = actuator_effort_;
auto & joint_eff = joint_effort_;
if ( act_eff.size( ) == num_actuators( ) && joint_eff.size( ) == num_joints( ) )
{
assert( act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1] );
joint_eff[0].set_value(
jr[0] * ( act_eff[0].get_value( ) * ar[0] + act_eff[1].get_value( ) * ar[1] ) );
joint_eff[1].set_value(
jr[1] * ( act_eff[0].get_value( ) * ar[0] - act_eff[1].get_value( ) * ar[1] ) );
}
}
inline void DifferentialTransmission::joint_to_actuator( )
{
const auto & ar = actuator_reduction_;
const auto & jr = joint_reduction_;
auto & act_pos = actuator_position_;
auto & joint_pos = joint_position_;
if ( act_pos.size( ) == num_actuators( ) && joint_pos.size( ) == num_joints( ) )
{
assert( act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1] );
double joints_offset_applied[2] = {
joint_pos[0].get_value( ) - joint_offset_[0], joint_pos[1].get_value( ) - joint_offset_[1]};
act_pos[0].set_value(
( joints_offset_applied[0] * jr[0] + joints_offset_applied[1] * jr[1] ) * ar[0] );
act_pos[1].set_value(
( joints_offset_applied[0] * jr[0] - joints_offset_applied[1] * jr[1] ) * ar[1] );
}
auto & act_vel = actuator_velocity_;
auto & joint_vel = joint_velocity_;
if ( act_vel.size( ) == num_actuators( ) && joint_vel.size( ) == num_joints( ) )
{
assert( act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1] );
act_vel[0].set_value(
( joint_vel[0].get_value( ) * jr[0] + joint_vel[1].get_value( ) * jr[1] ) * ar[0] );
act_vel[1].set_value(
( joint_vel[0].get_value( ) * jr[0] - joint_vel[1].get_value( ) * jr[1] ) * ar[1] );
}
auto & act_eff = actuator_effort_;
auto & joint_eff = joint_effort_;
if ( act_eff.size( ) == num_actuators( ) && joint_eff.size( ) == num_joints( ) )
{
assert( act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1] );
act_eff[0].set_value(
( joint_eff[0].get_value( ) / jr[0] + joint_eff[1].get_value( ) / jr[1] ) / ( 2.0 * ar[0] ) );
act_eff[1].set_value(
( joint_eff[0].get_value( ) / jr[0] - joint_eff[1].get_value( ) / jr[1] ) / ( 2.0 * ar[1] ) );
}
}
std::string DifferentialTransmission::get_handles_info( ) const
{
return std::string( "Got the following handles:\n" ) +
"Joint position: " + to_string( get_names( joint_position_ ) ) +
", Actuator position: " + to_string( get_names( actuator_position_ ) ) + "\n" +
"Joint velocity: " + to_string( get_names( joint_velocity_ ) ) +
", Actuator velocity: " + to_string( get_names( actuator_velocity_ ) ) + "\n" +
"Joint effort: " + to_string( get_names( joint_effort_ ) ) +
", Actuator effort: " + to_string( get_names( actuator_effort_ ) );
}
} // namespace transmission_interface
#endif // TRANSMISSION_INTERFACE__DIFFERENTIAL_TRANSMISSION_HPP_
1 // Copyright 2022 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TRANSMISSION_INTERFACE__DIFFERENTIAL_TRANSMISSION_LOADER_HPP_
#define TRANSMISSION_INTERFACE__DIFFERENTIAL_TRANSMISSION_LOADER_HPP_
#include <memory>
#include "transmission_interface/transmission.hpp"
#include "transmission_interface/transmission_loader.hpp"
namespace transmission_interface
{
/**
* \brief Class for loading a four-bar linkage transmission instance from configuration data.
*/
28 class DifferentialTransmissionLoader : public TransmissionLoader
{
public:
std::shared_ptr<Transmission> load(
const hardware_interface::TransmissionInfo & transmission_info ) override;
};
} // namespace transmission_interface
#endif // TRANSMISSION_INTERFACE__DIFFERENTIAL_TRANSMISSION_LOADER_HPP_
// Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TRANSMISSION_INTERFACE__EXCEPTION_HPP_
#define TRANSMISSION_INTERFACE__EXCEPTION_HPP_
#include <exception>
#include <string>
namespace transmission_interface
{
22 class Exception : public std::exception
{
public:
25 explicit Exception( const char * message ) : msg( message ) {}
26 explicit Exception( const std::string & message ) : msg( message ) {}
const char * what( ) const noexcept override { return msg.c_str( ); }
private:
std::string msg;
};
} // namespace transmission_interface
#endif // TRANSMISSION_INTERFACE__EXCEPTION_HPP_
// Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/// \author Adolfo Rodriguez Tsouroukdissian
#ifndef TRANSMISSION_INTERFACE__FOUR_BAR_LINKAGE_TRANSMISSION_HPP_
#define TRANSMISSION_INTERFACE__FOUR_BAR_LINKAGE_TRANSMISSION_HPP_
#include <cassert>
#include <string>
#include <vector>
#include "transmission_interface/accessor.hpp"
#include "transmission_interface/exception.hpp"
#include "transmission_interface/transmission.hpp"
namespace transmission_interface
{
/// Implementation of a four-bar-linkage transmission.
/**
*
* This transmission relates <b>two actuators</b> and <b>two joints</b> through a mechanism in which the state of the
* first joint only depends on the first actuator, while the second joint depends on both actuators, as
* illustrated below.
* Although the class name makes specific reference to the four-bar-linkage, there are other mechanical layouts
* that yield the same behavior, such as the remote actuation example also depicted below.
* \image html four_bar_linkage_transmission.png
*
* <CENTER>
* <table>
* <tr><th></th><th><CENTER>Effort</CENTER></th><th><CENTER>Velocity</CENTER></th><th><CENTER>Position</CENTER></th></tr>
* <tr><td>
* <b> Actuator to joint </b>
* </td>
* <td>
* \f{eqnarray*}{
* \tau_{j_1} & = & n_{j_1} n_{a_1} \tau_{a_1} \\
* \tau_{j_2} & = & n_{j_2} ( n_{a_2} \tau_{a_2} - n_{j_1} n_{a_1} \tau_{a_1} )
* \f}
* </td>
* <td>
* \f{eqnarray*}{
* \dot{x}_{j_1} & = & \frac{ \dot{x}_{a_1} }{ n_{j_1} n_{a_1} } \\
* \dot{x}_{j_2} & = & \frac{ \dot{x}_{a_2} / n_{a_2} - \dot{x}_{a_1} / ( n_{j_1} n_{a_1} ) }{ n_{j_2} }
* \f}
* </td>
* <td>
* \f{eqnarray*}{
* x_{j_1} & = & \frac{ x_{a_1} }{ n_{j_1} n_{a_1} } + x_{off_1} \\
* x_{j_2} & = & \frac{ x_{a_2} / n_{a_2} - x_{a_1} / ( n_{j_1} n_{a_1} ) }{ n_{j_2} } + x_{off_2}
* \f}
* </td>
* </tr>
* <tr><td>
* <b> Joint to actuator </b>
* </td>
* <td>
* \f{eqnarray*}{
* \tau_{a_1} & = & \tau_{j_1} / ( n_{j_1} n_{a_1} ) \\
* \tau_{a_2} & = & \frac{ \tau_{j_1} + \tau_{j_2} / n_{j_2} }{ n_{a_2} }
* \f}
* </td>
* <td>
* \f{eqnarray*}{
* \dot{x}_{a_1} & = & n_{j_1} n_{a_1} \dot{x}_{j_1} \\
* \dot{x}_{a_2} & = & n_{a_2} ( \dot{x}_{j_1} + n_{j_2} \dot{x}_{j_2} )
* \f}
* </td>
* <td>
* \f{eqnarray*}{
* x_{a_1} & = & n_{j_1} n_{a_1} ( x_{j_1} - x_{off_1} ) \\
* x_{a_2} & = & n_{a_2} \left[( x_{j_1} - x_{off_1} ) + n_{j_2} ( x_{j_2} - x_{off_2} )\right]
* \f}
* </td></tr></table>
* </CENTER>
*
* where:
* - \f$ x \f$, \f$ \dot{x} \f$ and \f$ \tau \f$ are position, velocity and effort variables, respectively.
* - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space variables, respectively.
* - \f$ x_{off}\f$ represents the offset between motor and joint zeros, expressed in joint position coordinates.
* ( cf. SimpleTransmission class documentation for a more detailed description of this variable ).
* - \f$ n \f$ represents a transmission ratio ( reducers/amplifiers are depicted as timing belts in the figure ).
* A transmission ratio can take any real value \e except zero. In particular:
* - If its absolute value is greater than one, it's a velocity reducer / effort amplifier, while if its absolute
* value lies in \f$ ( 0, 1 ) \f$ it's a velocity amplifier / effort reducer.
* - Negative values represent a direction flip, ie. input and output move in opposite directions.
* - <b>Important:</b> Use transmission ratio signs to match this class' convention of positive actuator/joint
* directions with a given mechanical design, as they will in general not match.
*
* \ingroup transmission_types
*/
103 class FourBarLinkageTransmission : public Transmission
{
public:
/**
* \param actuator_reduction Reduction ratio of actuators.
* \param joint_reduction Reduction ratio of joints.
* \param joint_offset Joint position offset used in the position mappings.
* \pre Nonzero actuator reduction values.
*/
112 FourBarLinkageTransmission(
const std::vector<double> & actuator_reduction, const std::vector<double> & joint_reduction,
const std::vector<double> & joint_offset = std::vector<double>( 2, 0.0 ) );
/**
* \param[in] joint_handles Handles of joint values.
* \param[in] actuator_handles Handles of actuator values.
* \pre Handles are valid and matching in size
*/
void configure(
const std::vector<JointHandle> & joint_handles,
const std::vector<ActuatorHandle> & actuator_handles ) override;
/// Transform variables from actuator to joint space.
/**
* \pre Actuator and joint vectors must have size 2 and point to valid data.
* To call this method it is not required that all other data vectors contain valid data, and can even remain empty.
*/
void actuator_to_joint( ) override;
/// Transform variables from joint to actuator space.
/**
* \pre Actuator and joint vectors must have size 2 and point to valid data.
* To call this method it is not required that all other data vectors contain valid data, and can even remain empty.
*/
void joint_to_actuator( ) override;
std::size_t num_actuators( ) const override { return 2; }
140 std::size_t num_joints( ) const override { return 2; }
142 const std::vector<double> & get_actuator_reduction( ) const { return actuator_reduction_; }
143 const std::vector<double> & get_joint_reduction( ) const { return joint_reduction_; }
144 const std::vector<double> & get_joint_offset( ) const { return joint_offset_; }
/// Get human-friendly report of handles
147 std::string get_handles_info( ) const;
protected:
std::vector<double> actuator_reduction_;
std::vector<double> joint_reduction_;
std::vector<double> joint_offset_;
std::vector<JointHandle> joint_position_;
std::vector<JointHandle> joint_velocity_;
std::vector<JointHandle> joint_effort_;
std::vector<ActuatorHandle> actuator_position_;
std::vector<ActuatorHandle> actuator_velocity_;
std::vector<ActuatorHandle> actuator_effort_;
};
inline FourBarLinkageTransmission::FourBarLinkageTransmission(
const std::vector<double> & actuator_reduction, const std::vector<double> & joint_reduction,
const std::vector<double> & joint_offset )
: actuator_reduction_( actuator_reduction ),
joint_reduction_( joint_reduction ),
joint_offset_( joint_offset )
{
if (
num_actuators( ) != actuator_reduction_.size( ) || num_joints( ) != joint_reduction_.size( ) ||
num_joints( ) != joint_offset_.size( ) )
{
throw Exception( "Reduction and offset vectors must have size 2." );
}
if (
0.0 == actuator_reduction_[0] || 0.0 == actuator_reduction_[1] || 0.0 == joint_reduction_[0] ||
0.0 == joint_reduction_[1] )
{
throw Exception( "Transmission reduction ratios cannot be zero." );
}
}
void FourBarLinkageTransmission::configure(
const std::vector<JointHandle> & joint_handles,
const std::vector<ActuatorHandle> & actuator_handles )
{
if ( joint_handles.empty( ) )
{
throw Exception( "No joint handles were passed in" );
}
if ( actuator_handles.empty( ) )
{
throw Exception( "No actuator handles were passed in" );
}
const auto joint_names = get_names( joint_handles );
if ( joint_names.size( ) != 2 )
{
throw Exception(
"There should be exactly two unique joint names but was given " + to_string( joint_names ) );
}
const auto actuator_names = get_names( actuator_handles );
if ( actuator_names.size( ) != 2 )
{
throw Exception(
"There should be exactly two unique actuator names but was given " +
to_string( actuator_names ) );
}
joint_position_ =
get_ordered_handles( joint_handles, joint_names, hardware_interface::HW_IF_POSITION );
joint_velocity_ =
get_ordered_handles( joint_handles, joint_names, hardware_interface::HW_IF_VELOCITY );
joint_effort_ = get_ordered_handles( joint_handles, joint_names, hardware_interface::HW_IF_EFFORT );
if ( joint_position_.size( ) != 2 && joint_velocity_.size( ) != 2 && joint_effort_.size( ) != 2 )
{
throw Exception( "Not enough valid or required joint handles were presented." );
}
actuator_position_ =
get_ordered_handles( actuator_handles, actuator_names, hardware_interface::HW_IF_POSITION );
actuator_velocity_ =
get_ordered_handles( actuator_handles, actuator_names, hardware_interface::HW_IF_VELOCITY );
actuator_effort_ =
get_ordered_handles( actuator_handles, actuator_names, hardware_interface::HW_IF_EFFORT );
if (
actuator_position_.size( ) != 2 && actuator_velocity_.size( ) != 2 &&
actuator_effort_.size( ) != 2 )
{
throw Exception(
"Not enough valid or required actuator handles were presented. \n" + get_handles_info( ) );
}
if (
joint_position_.size( ) != actuator_position_.size( ) &&
joint_velocity_.size( ) != actuator_velocity_.size( ) &&
joint_effort_.size( ) != actuator_effort_.size( ) )
{
throw Exception( "Pair-wise mismatch on interfaces. \n" + get_handles_info( ) );
}
}
inline void FourBarLinkageTransmission::actuator_to_joint( )
{
const auto & ar = actuator_reduction_;
const auto & jr = joint_reduction_;
// position
auto & act_pos = actuator_position_;
auto & joint_pos = joint_position_;
if ( act_pos.size( ) == num_actuators( ) && joint_pos.size( ) == num_joints( ) )
{
assert( act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1] );
joint_pos[0].set_value( act_pos[0].get_value( ) / ( jr[0] * ar[0] ) + joint_offset_[0] );
joint_pos[1].set_value(
( act_pos[1].get_value( ) / ar[1] - act_pos[0].get_value( ) / ( jr[0] * ar[0] ) ) / jr[1] +
joint_offset_[1] );
}
// velocity
auto & act_vel = actuator_velocity_;
auto & joint_vel = joint_velocity_;
if ( act_vel.size( ) == num_actuators( ) && joint_vel.size( ) == num_joints( ) )
{
assert( act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1] );
joint_vel[0].set_value( act_vel[0].get_value( ) / ( jr[0] * ar[0] ) );
joint_vel[1].set_value(
( act_vel[1].get_value( ) / ar[1] - act_vel[0].get_value( ) / ( jr[0] * ar[0] ) ) / jr[1] );
}
// effort
auto & act_eff = actuator_effort_;
auto & joint_eff = joint_effort_;
if ( act_eff.size( ) == num_actuators( ) && joint_eff.size( ) == num_joints( ) )
{
assert( act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1] );
joint_eff[0].set_value( jr[0] * act_eff[0].get_value( ) * ar[0] );
joint_eff[1].set_value(
jr[1] * ( act_eff[1].get_value( ) * ar[1] - act_eff[0].get_value( ) * ar[0] * jr[0] ) );
}
}
inline void FourBarLinkageTransmission::joint_to_actuator( )
{
const auto & ar = actuator_reduction_;
const auto & jr = joint_reduction_;
// position
auto & act_pos = actuator_position_;
auto & joint_pos = joint_position_;
if ( act_pos.size( ) == num_actuators( ) && joint_pos.size( ) == num_joints( ) )
{
assert( act_pos[0] && act_pos[1] && joint_pos[0] && joint_pos[1] );
double joints_offset_applied[2] = {
joint_pos[0].get_value( ) - joint_offset_[0], joint_pos[1].get_value( ) - joint_offset_[1]};
act_pos[0].set_value( joints_offset_applied[0] * jr[0] * ar[0] );
act_pos[1].set_value( ( joints_offset_applied[0] + joints_offset_applied[1] * jr[1] ) * ar[1] );
}
// velocity
auto & act_vel = actuator_velocity_;
auto & joint_vel = joint_velocity_;
if ( act_vel.size( ) == num_actuators( ) && joint_vel.size( ) == num_joints( ) )
{
assert( act_vel[0] && act_vel[1] && joint_vel[0] && joint_vel[1] );
act_vel[0].set_value( joint_vel[0].get_value( ) * jr[0] * ar[0] );
act_vel[1].set_value( ( joint_vel[0].get_value( ) + joint_vel[1].get_value( ) * jr[1] ) * ar[1] );
}
// effort
auto & act_eff = actuator_effort_;
auto & joint_eff = joint_effort_;
if ( act_eff.size( ) == num_actuators( ) && joint_eff.size( ) == num_joints( ) )
{
assert( act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1] );
act_eff[0].set_value( joint_eff[0].get_value( ) / ( ar[0] * jr[0] ) );
act_eff[1].set_value( ( joint_eff[0].get_value( ) + joint_eff[1].get_value( ) / jr[1] ) / ar[1] );
}
}
std::string FourBarLinkageTransmission::get_handles_info( ) const
{
return std::string( "Got the following handles:\n" ) +
"Joint position: " + to_string( get_names( joint_position_ ) ) +
", Actuator position: " + to_string( get_names( actuator_position_ ) ) + "\n" +
"Joint velocity: " + to_string( get_names( joint_velocity_ ) ) +
", Actuator velocity: " + to_string( get_names( actuator_velocity_ ) ) + "\n" +
"Joint effort: " + to_string( get_names( joint_effort_ ) ) +
", Actuator effort: " + to_string( get_names( actuator_effort_ ) );
}
} // namespace transmission_interface
#endif // TRANSMISSION_INTERFACE__FOUR_BAR_LINKAGE_TRANSMISSION_HPP_
1 // Copyright 2022 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TRANSMISSION_INTERFACE__FOUR_BAR_LINKAGE_TRANSMISSION_LOADER_HPP_
#define TRANSMISSION_INTERFACE__FOUR_BAR_LINKAGE_TRANSMISSION_LOADER_HPP_
#include <memory>
#include "transmission_interface/transmission.hpp"
#include "transmission_interface/transmission_loader.hpp"
namespace transmission_interface
{
/**
* \brief Class for loading a four-bar linkage transmission instance from configuration data.
*/
28 class FourBarLinkageTransmissionLoader : public TransmissionLoader
{
public:
std::shared_ptr<Transmission> load(
const hardware_interface::TransmissionInfo & transmission_info ) override;
};
} // namespace transmission_interface
#endif // TRANSMISSION_INTERFACE__FOUR_BAR_LINKAGE_TRANSMISSION_LOADER_HPP_
1 // Copyright 2020 ros2_control development team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TRANSMISSION_INTERFACE__HANDLE_HPP_
#define TRANSMISSION_INTERFACE__HANDLE_HPP_
#include <string>
#include "hardware_interface/handle.hpp"
namespace transmission_interface
{
/** A handle used to get and set a value on a given actuator interface. */
25 class ActuatorHandle : public hardware_interface::ReadWriteHandle
{
public:
using hardware_interface::ReadWriteHandle::ReadWriteHandle;
};
/** A handle used to get and set a value on a given joint interface. */
32 class JointHandle : public hardware_interface::ReadWriteHandle
{
public:
using hardware_interface::ReadWriteHandle::ReadWriteHandle;
};
} // namespace transmission_interface
#endif // TRANSMISSION_INTERFACE__HANDLE_HPP_
// Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TRANSMISSION_INTERFACE__SIMPLE_TRANSMISSION_HPP_
#define TRANSMISSION_INTERFACE__SIMPLE_TRANSMISSION_HPP_
#include <algorithm>
#include <cassert>
#include <string>
#include <vector>
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "transmission_interface/exception.hpp"
#include "transmission_interface/transmission.hpp"
namespace transmission_interface
{
/// Implementation of a simple reducer transmission.
/**
* This transmission relates <b>one actuator</b> and <b>one joint</b> through a reductor ( or amplifier ).
* Timing belts and gears are examples of this transmission type, and are illustrated below.
* \image html simple_transmission.png
*
* <CENTER>
* <table>
* <tr><th></th><th><CENTER>Effort</CENTER></th><th><CENTER>Velocity</CENTER></th><th><CENTER>Position</CENTER></th></tr>
* <tr><td>
* <b> Actuator to joint </b>
* </td>
* <td>
* \f[ \tau_j = n \tau_a \f]
* </td>
* <td>
* \f[ \dot{x}_j = \dot{x}_a / n \f]
* </td>
* <td>
* \f[ x_j = x_a / n + x_{off} \f]
* </td>
* </tr>
* <tr><td>
* <b> Joint to actuator </b>
* </td>
* <td>
* \f[ \tau_a = \tau_j / n\f]
* </td>
* <td>
* \f[ \dot{x}_a = n \dot{x}_j \f]
* </td>
* <td>
* \f[ x_a = n ( x_j - x_{off} ) \f]
* </td></tr></table>
* </CENTER>
*
* where:
* - \f$ x \f$, \f$ \dot{x} \f$ and \f$ \tau \f$ are position, velocity and effort variables, respectively.
* - Subindices \f$ _a \f$ and \f$ _j \f$ are used to represent actuator-space and joint-space variables, respectively.
* - \f$ x_{off}\f$ represents the offset between motor and joint zeros, expressed in joint position coordinates.
* - \f$ n \f$ is the transmission ratio, and can be computed as the ratio between the output and input pulley
* radii for the timing belt; or the ratio between output and input teeth for the gear system.
* The transmission ratio can take any real value \e except zero. In particular:
* - If its absolute value is greater than one, it's a velocity reducer / effort amplifier, while if its absolute
* value lies in \f$ ( 0, 1 ) \f$ it's a velocity amplifier / effort reducer.
* - Negative values represent a direction flip, ie. actuator and joint move in opposite directions. For example,
* in timing belts actuator and joint move in the same direction, while in single-stage gear systems actuator and
* joint move in opposite directions.
*
* \ingroup transmission_types
*/
80 class SimpleTransmission : public Transmission
{
public:
/**
* \param[in] joint_to_actuator_reduction Joint to actuator reduction ratio.
* \param[in] joint_offset Joint position offset used in the position mappings.
* \pre Nonzero reduction value.
*/
88 explicit SimpleTransmission(
const double joint_to_actuator_reduction, const double joint_offset = 0.0 );
/// Set up the data the transmission operates on.
/**
* \param[in] joint_handles Vector of interface handles of one joint
* \param[in] actuator_handles Vector of interface handles of one actuator
* \pre Vectors are nonzero.
* \pre Joint handles share the same joint name and actuator handles share the same actuator name.
*/
void configure(
const std::vector<JointHandle> & joint_handles,
const std::vector<ActuatorHandle> & actuator_handles ) override;
/// Transform variables from actuator to joint space.
/**
* This method operates on the handles provided when configuring the transmission.
* To call this method it is not required that all supported interface types are provided, e.g. one can supply only velocity handles
*/
void actuator_to_joint( ) override;
/// Transform variables from joint to actuator space.
/**
* This method operates on the handles provided when configuring the transmission.
* To call this method it is not required that all supported interface types are provided, e.g. one can supply only velocity handles
*/
void joint_to_actuator( ) override;
std::size_t num_actuators( ) const override { return 1; }
117 std::size_t num_joints( ) const override { return 1; }
119 double get_actuator_reduction( ) const { return reduction_; }
120 double get_joint_offset( ) const { return jnt_offset_; }
protected:
double reduction_;
double jnt_offset_;
JointHandle joint_position_ = {"", "", nullptr};
JointHandle joint_velocity_ = {"", "", nullptr};
JointHandle joint_effort_ = {"", "", nullptr};
ActuatorHandle actuator_position_ = {"", "", nullptr};
ActuatorHandle actuator_velocity_ = {"", "", nullptr};
ActuatorHandle actuator_effort_ = {"", "", nullptr};
};
inline SimpleTransmission::SimpleTransmission(
const double joint_to_actuator_reduction, const double joint_offset )
: reduction_( joint_to_actuator_reduction ), jnt_offset_( joint_offset )
{
if ( reduction_ == 0.0 )
{
throw Exception( "Transmission reduction ratio cannot be zero." );
}
}
template <class HandleType>
HandleType get_by_interface(
const std::vector<HandleType> & handles, const std::string & interface_name )
{
const auto result = std::find_if(
handles.cbegin( ), handles.cend( ),
[&interface_name]( const auto handle ) { return handle.get_interface_name( ) == interface_name; } );
if ( result == handles.cend( ) )
{
return HandleType( handles.cbegin( )->get_prefix_name( ), interface_name, nullptr );
}
return *result;
}
template <class T>
bool are_names_identical( const std::vector<T> & handles )
{
std::vector<std::string> names;
std::transform(
handles.cbegin( ), handles.cend( ), std::back_inserter( names ),
[]( const auto & handle ) { return handle.get_prefix_name( ); } );
return std::equal( names.cbegin( ) + 1, names.cend( ), names.cbegin( ) );
}
inline void SimpleTransmission::configure(
const std::vector<JointHandle> & joint_handles,
const std::vector<ActuatorHandle> & actuator_handles )
{
if ( joint_handles.empty( ) )
{
throw Exception( "No joint handles were passed in" );
}
if ( actuator_handles.empty( ) )
{
throw Exception( "No actuator handles were passed in" );
}
if ( !are_names_identical( joint_handles ) )
{
throw Exception( "Joint names given to transmissions should be identical" );
}
if ( !are_names_identical( actuator_handles ) )
{
throw Exception( "Actuator names given to transmissions should be identical" );
}
joint_position_ = get_by_interface( joint_handles, hardware_interface::HW_IF_POSITION );
joint_velocity_ = get_by_interface( joint_handles, hardware_interface::HW_IF_VELOCITY );
joint_effort_ = get_by_interface( joint_handles, hardware_interface::HW_IF_EFFORT );
if ( !joint_position_ && !joint_velocity_ && !joint_effort_ )
{
throw Exception( "None of the provided joint handles are valid or from the required interfaces" );
}
actuator_position_ = get_by_interface( actuator_handles, hardware_interface::HW_IF_POSITION );
actuator_velocity_ = get_by_interface( actuator_handles, hardware_interface::HW_IF_VELOCITY );
actuator_effort_ = get_by_interface( actuator_handles, hardware_interface::HW_IF_EFFORT );
if ( !actuator_position_ && !actuator_velocity_ && !actuator_effort_ )
{
throw Exception( "None of the provided joint handles are valid or from the required interfaces" );
}
}
inline void SimpleTransmission::actuator_to_joint( )
{
if ( joint_effort_ && actuator_effort_ )
{
joint_effort_.set_value( actuator_effort_.get_value( ) * reduction_ );
}
if ( joint_velocity_ && actuator_velocity_ )
{
joint_velocity_.set_value( actuator_velocity_.get_value( ) / reduction_ );
}
if ( joint_position_ && actuator_position_ )
{
joint_position_.set_value( actuator_position_.get_value( ) / reduction_ + jnt_offset_ );
}
}
inline void SimpleTransmission::joint_to_actuator( )
{
if ( joint_effort_ && actuator_effort_ )
{
actuator_effort_.set_value( joint_effort_.get_value( ) / reduction_ );
}
if ( joint_velocity_ && actuator_velocity_ )
{
actuator_velocity_.set_value( joint_velocity_.get_value( ) * reduction_ );
}
if ( joint_position_ && actuator_position_ )
{
actuator_position_.set_value( ( joint_position_.get_value( ) - jnt_offset_ ) * reduction_ );
}
}
} // namespace transmission_interface
#endif // TRANSMISSION_INTERFACE__SIMPLE_TRANSMISSION_HPP_
1 // Copyright 2022 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TRANSMISSION_INTERFACE__SIMPLE_TRANSMISSION_LOADER_HPP_
#define TRANSMISSION_INTERFACE__SIMPLE_TRANSMISSION_LOADER_HPP_
#include <memory>
#include <transmission_interface/transmission.hpp>
#include <transmission_interface/transmission_loader.hpp>
namespace transmission_interface
{
/**
* \brief Class for loading a simple transmission instance from configuration data.
*/
28 class SimpleTransmissionLoader : public TransmissionLoader
{
public:
std::shared_ptr<Transmission> load(
const hardware_interface::TransmissionInfo & transmission_info ) override;
};
} // namespace transmission_interface
#endif // TRANSMISSION_INTERFACE__SIMPLE_TRANSMISSION_LOADER_HPP_
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TRANSMISSION_INTERFACE__TRANSMISSION_HPP_
#define TRANSMISSION_INTERFACE__TRANSMISSION_HPP_
#include <cstddef>
#include <memory>
#include <stdexcept>
#include <string>
#include <vector>
#include "transmission_interface/handle.hpp"
namespace transmission_interface
{
/// Abstract base class for representing mechanical transmissions.
/**
* Mechanical transmissions transform effort/flow variables such that their product ( power ) remains constant.
* Effort variables for linear and rotational domains are \e force and \e torque; while the flow variables are
* respectively linear velocity and angular velocity.
*
* In robotics it is customary to place transmissions between actuators and joints. This interface adheres to this
* naming to identify the input and output spaces of the transformation.
* The provided interfaces allow bidirectional mappings between actuator and joint spaces for effort, velocity and
* position. Position is not a power variable, but the mappings can be implemented using the velocity map plus an
* integration constant representing the offset between actuator and joint zeros.
*
* \par Credit
* This interface was inspired by similar existing implementations by PAL Robotics, S.L. and Willow Garage Inc.
*
* \note Implementations of this interface must take care of realtime-safety if the code is to be run in realtime
* contexts, as is often the case in robot control.
*/
45 class Transmission
{
public:
virtual ~Transmission( ) = default;
virtual void configure(
const std::vector<JointHandle> & joint_handles,
const std::vector<ActuatorHandle> & actuator_handles ) = 0;
/// Transform \e effort variables from actuator to joint space.
/**
* \param[in] act_data Actuator-space variables.
* \param[out] jnt_data Joint-space variables.
* \pre All non-empty vectors must contain valid data and their size should be consistent with the number of
* transmission actuators and joints.
* Data vectors not used in this map can remain empty.
*/
virtual void actuator_to_joint( ) = 0;
/// Transform \e effort variables from joint to actuator space.
/**
* \param[in] jnt_data Joint-space variables.
* \param[out] act_data Actuator-space variables.
* \pre All non-empty vectors must contain valid data and their size should be consistent with the number of
* transmission actuators and joints.
* Data vectors not used in this map can remain empty.
*/
virtual void joint_to_actuator( ) = 0;
/** \return Number of actuators managed by transmission,
* ie. the dimension of the actuator space.
*/
virtual std::size_t num_actuators( ) const = 0;
/** \return Number of joints managed by transmission,
* ie. the dimension of the joint space.
*/
virtual std::size_t num_joints( ) const = 0;
};
typedef std::shared_ptr<Transmission> TransmissionSharedPtr;
} // namespace transmission_interface
#endif // TRANSMISSION_INTERFACE__TRANSMISSION_HPP_
// Copyright 2022 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TRANSMISSION_INTERFACE__TRANSMISSION_INTERFACE_EXCEPTION_HPP_
#define TRANSMISSION_INTERFACE__TRANSMISSION_INTERFACE_EXCEPTION_HPP_
#include <exception>
#include <string>
namespace transmission_interface
{
23 class TransmissionInterfaceException : public std::exception
{
public:
26 explicit TransmissionInterfaceException( const std::string & message ) : msg( message ) {}
const char * what( ) const noexcept override { return msg.c_str( ); }
private:
std::string msg;
};
} // namespace transmission_interface
#endif // TRANSMISSION_INTERFACE__TRANSMISSION_INTERFACE_EXCEPTION_HPP_
1 // Copyright 2022 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TRANSMISSION_INTERFACE__TRANSMISSION_LOADER_HPP_
#define TRANSMISSION_INTERFACE__TRANSMISSION_LOADER_HPP_
#include <memory>
#include "hardware_interface/hardware_info.hpp"
namespace transmission_interface
{
/**
* \brief Abstract interface for loading transmission instances from configuration data.
*
* It also provides convenience methods for specific transmission loaders to leverage.
*/
29 class TransmissionLoader
{
public:
virtual ~TransmissionLoader( ) = default;
virtual std::shared_ptr<Transmission> load(
const hardware_interface::TransmissionInfo & transmission_info ) = 0;
};
} // namespace transmission_interface
#endif // TRANSMISSION_INTERFACE__TRANSMISSION_LOADER_HPP_
1 // Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TRANSMISSION_INTERFACE__VISIBILITY_CONTROL_H_
#define TRANSMISSION_INTERFACE__VISIBILITY_CONTROL_H_
// This logic was borrowed ( then namespaced ) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define TRANSMISSION_INTERFACE_EXPORT __attribute__( ( dllexport ) )
#define TRANSMISSION_INTERFACE_IMPORT __attribute__( ( dllimport ) )
#else
#define TRANSMISSION_INTERFACE_EXPORT __declspec( dllexport )
#define TRANSMISSION_INTERFACE_IMPORT __declspec( dllimport )
#endif
#ifdef TRANSMISSION_INTERFACE_BUILDING_DLL
#define TRANSMISSION_INTERFACE_PUBLIC TRANSMISSION_INTERFACE_EXPORT
#else
#define TRANSMISSION_INTERFACE_PUBLIC TRANSMISSION_INTERFACE_IMPORT
#endif
#define TRANSMISSION_INTERFACE_PUBLIC_TYPE TRANSMISSION_INTERFACE_PUBLIC
#define TRANSMISSION_INTERFACE_LOCAL
#else
#define TRANSMISSION_INTERFACE_EXPORT __attribute__( ( visibility( "default" ) ) )
#define TRANSMISSION_INTERFACE_IMPORT
#if __GNUC__ >= 4
#define TRANSMISSION_INTERFACE_PUBLIC __attribute__( ( visibility( "default" ) ) )
#define TRANSMISSION_INTERFACE_LOCAL __attribute__( ( visibility( "hidden" ) ) )
#else
#define TRANSMISSION_INTERFACE_PUBLIC
#define TRANSMISSION_INTERFACE_LOCAL
#endif
#define TRANSMISSION_INTERFACE_PUBLIC_TYPE
#endif
#endif // TRANSMISSION_INTERFACE__VISIBILITY_CONTROL_H_
1 // Copyright 2022 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "transmission_interface/differential_transmission_loader.hpp"
#include <memory>
#include "hardware_interface/component_parser.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/rclcpp.hpp"
#include "transmission_interface/differential_transmission.hpp"
namespace transmission_interface
{
29 std::shared_ptr<Transmission> DifferentialTransmissionLoader::load(
const hardware_interface::TransmissionInfo & transmission_info )
{
try
{
const auto act_reduction1 = transmission_info.actuators.at( 0 ).mechanical_reduction;
const auto act_reduction2 = transmission_info.actuators.at( 1 ).mechanical_reduction;
const auto jnt_reduction1 = transmission_info.joints.at( 0 ).mechanical_reduction;
const auto jnt_reduction2 = transmission_info.joints.at( 1 ).mechanical_reduction;
const auto jnt_offset1 = transmission_info.joints.at( 0 ).offset;
const auto jnt_offset2 = transmission_info.joints.at( 1 ).offset;
std::shared_ptr<Transmission> transmission( new DifferentialTransmission(
{act_reduction1, act_reduction2}, {jnt_reduction1, jnt_reduction2},
{jnt_offset1, jnt_offset2} ) );
return transmission;
}
catch ( const std::exception & ex )
{
RCLCPP_ERROR(
rclcpp::get_logger( "differential_transmission_loader" ),
"Failed to construct transmission '%s'", ex.what( ) );
return std::shared_ptr<Transmission>( );
}
}
} // namespace transmission_interface
59 PLUGINLIB_EXPORT_CLASS(
transmission_interface::DifferentialTransmissionLoader,
transmission_interface::TransmissionLoader )
1 // Copyright 2022 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "transmission_interface/four_bar_linkage_transmission_loader.hpp"
#include <memory>
#include "hardware_interface/component_parser.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/rclcpp.hpp"
#include "transmission_interface/four_bar_linkage_transmission.hpp"
namespace transmission_interface
{
29 std::shared_ptr<Transmission> FourBarLinkageTransmissionLoader::load(
const hardware_interface::TransmissionInfo & transmission_info )
{
try
{
const auto act_reduction1 = transmission_info.actuators.at( 0 ).mechanical_reduction;
const auto act_reduction2 = transmission_info.actuators.at( 1 ).mechanical_reduction;
const auto jnt_reduction1 = transmission_info.joints.at( 0 ).mechanical_reduction;
const auto jnt_reduction2 = transmission_info.joints.at( 1 ).mechanical_reduction;
const auto jnt_offset1 = transmission_info.joints.at( 0 ).offset;
const auto jnt_offset2 = transmission_info.joints.at( 1 ).offset;
std::shared_ptr<Transmission> transmission( new FourBarLinkageTransmission(
{act_reduction1, act_reduction2}, {jnt_reduction1, jnt_reduction2},
{jnt_offset1, jnt_offset2} ) );
return transmission;
}
catch ( const std::exception & ex )
{
RCLCPP_ERROR(
rclcpp::get_logger( "four_bar_linkage_transmission_loader" ),
"Failed to construct transmission '%s'", ex.what( ) );
return std::shared_ptr<Transmission>( );
}
}
} // namespace transmission_interface
59 PLUGINLIB_EXPORT_CLASS(
transmission_interface::FourBarLinkageTransmissionLoader,
transmission_interface::TransmissionLoader )
1 // Copyright 2022 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "transmission_interface/simple_transmission_loader.hpp"
#include <memory>
#include "hardware_interface/component_parser.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "pluginlib/class_list_macros.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/rclcpp.hpp"
#include "transmission_interface/simple_transmission.hpp"
namespace transmission_interface
{
29 std::shared_ptr<Transmission> SimpleTransmissionLoader::load(
const hardware_interface::TransmissionInfo & transmission_info )
{
try
{
const auto mechanical_reduction = transmission_info.joints.at( 0 ).mechanical_reduction;
const auto offset = transmission_info.joints.at( 0 ).offset;
std::shared_ptr<Transmission> transmission(
new SimpleTransmission( mechanical_reduction, offset ) );
return transmission;
}
catch ( const std::exception & ex )
{
RCLCPP_ERROR(
rclcpp::get_logger( "simple_transmission_loader" ), "Failed to construct transmission '%s'",
ex.what( ) );
return std::shared_ptr<Transmission>( );
}
}
} // namespace transmission_interface
51 PLUGINLIB_EXPORT_CLASS(
transmission_interface::SimpleTransmissionLoader, transmission_interface::TransmissionLoader )
1 // Copyright 2022 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gmock/gmock.h>
#include <exception>
#include <memory>
#include <string>
#include <vector>
#include "hardware_interface/component_parser.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "pluginlib/class_loader.hpp"
#include "transmission_interface/differential_transmission.hpp"
#include "transmission_interface/differential_transmission_loader.hpp"
#include "transmission_interface/transmission_loader.hpp"
using testing::SizeIs;
31 class TransmissionPluginLoader
{
public:
34 std::shared_ptr<transmission_interface::TransmissionLoader> create( const std::string & type )
{
try
{
return class_loader_.createUniqueInstance( type );
}
catch ( std::exception & ex )
{
std::cerr << ex.what( ) << std::endl;
return std::shared_ptr<transmission_interface::TransmissionLoader>( );
}
}
private:
// must keep it alive because instance destroyers need it
49 pluginlib::ClassLoader<transmission_interface::TransmissionLoader> class_loader_ = {
"transmission_interface", "transmission_interface::TransmissionLoader"};
};
53 TEST( DifferentialTransmissionLoaderTest, FullSpec )
{
// Parse transmission info
std::string urdf_to_test = R"(
<?xml version="1.0"?>
<robot name="robot" xmlns="http://www.ros.org">
<ros2_control name="FullSpec" type="system">
<joint name="joint1">
<command_interface name="velocity">
<param name="min">-0.5</param>
<param name="max">0.5</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
<joint name="joint2">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
<transmission name="transmission1">
<plugin>transmission_interface/DifferentialTransmission</plugin>
<actuator name="joint1_motor" role="actuator1">
<mechanical_reduction>50</mechanical_reduction>
</actuator>
<actuator name="joint2_motor" role="actuator2">
<mechanical_reduction>-50</mechanical_reduction>
</actuator>
<joint name="joint1" role="joint1">
<offset>0.5</offset>
<mechanical_reduction>2.0</mechanical_reduction>
</joint>
<joint name="joint2" role="joint2">
<offset>-0.5</offset>
<mechanical_reduction>-2.0</mechanical_reduction>
</joint>
</transmission>
</ros2_control>
</robot>
)";
std::vector<hardware_interface::HardwareInfo> infos =
hardware_interface::parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( infos[0].transmissions, SizeIs( 1 ) );
// Transmission loader
TransmissionPluginLoader loader;
std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
loader.create( infos[0].transmissions[0].type );
ASSERT_TRUE( nullptr != transmission_loader );
std::shared_ptr<transmission_interface::Transmission> transmission;
const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
transmission = transmission_loader->load( info );
// Validate transmission
transmission_interface::DifferentialTransmission * differential_transmission =
dynamic_cast<transmission_interface::DifferentialTransmission *>( transmission.get( ) );
ASSERT_TRUE( nullptr != differential_transmission );
const std::vector<double> & actuator_reduction =
differential_transmission->get_actuator_reduction( );
EXPECT_EQ( 50.0, actuator_reduction[0] );
EXPECT_EQ( -50.0, actuator_reduction[1] );
const std::vector<double> & joint_reduction = differential_transmission->get_joint_reduction( );
EXPECT_EQ( 2.0, joint_reduction[0] );
EXPECT_EQ( -2.0, joint_reduction[1] );
const std::vector<double> & joint_offset = differential_transmission->get_joint_offset( );
EXPECT_EQ( 0.5, joint_offset[0] );
EXPECT_EQ( -0.5, joint_offset[1] );
}
TEST( DifferentialTransmissionLoaderTest, only_mech_red_specified )
{
// Parse transmission info
std::string urdf_to_test = R"(
<?xml version="1.0"?>
<robot name="robot" xmlns="http://www.ros.org">
<ros2_control name="FullSpec" type="system">
<joint name="joint1">
<command_interface name="velocity">
<param name="min">-0.5</param>
<param name="max">0.5</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
<joint name="joint2">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
<transmission name="transmission1">
<plugin>transmission_interface/DifferentialTransmission</plugin>
<actuator name="joint1_motor" role="actuator1">
<mechanical_reduction>50</mechanical_reduction>
</actuator>
<actuator name="joint2_motor" role="actuator2">
<mechanical_reduction>-50</mechanical_reduction>
</actuator>
<joint name="joint1" role="joint1">
<mechanical_reduction>1.0</mechanical_reduction>
</joint>
<joint name="joint2" role="joint2">
<mechanical_reduction>1.0</mechanical_reduction>
</joint>
</transmission>
</ros2_control>
</robot>
)";
std::vector<hardware_interface::HardwareInfo> infos =
hardware_interface::parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( infos[0].transmissions, SizeIs( 1 ) );
// Transmission loader
TransmissionPluginLoader loader;
std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
loader.create( infos[0].transmissions[0].type );
ASSERT_TRUE( nullptr != transmission_loader );
std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
transmission = transmission_loader->load( info );
// Validate transmission
transmission_interface::DifferentialTransmission * differential_transmission =
dynamic_cast<transmission_interface::DifferentialTransmission *>( transmission.get( ) );
const std::vector<double> & actuator_reduction =
differential_transmission->get_actuator_reduction( );
EXPECT_EQ( 50.0, actuator_reduction[0] );
EXPECT_EQ( -50.0, actuator_reduction[1] );
const std::vector<double> & joint_reduction = differential_transmission->get_joint_reduction( );
EXPECT_EQ( 1.0, joint_reduction[0] );
EXPECT_EQ( 1.0, joint_reduction[1] );
const std::vector<double> & joint_offset = differential_transmission->get_joint_offset( );
EXPECT_EQ( 0.0, joint_offset[0] );
EXPECT_EQ( 0.0, joint_offset[1] );
}
TEST( SimpleTransmissionLoaderTest, offset_and_mech_red_not_specified )
{
// Parse transmission info
std::string urdf_to_test = R"(
<?xml version="1.0"?>
<robot name="robot" xmlns="http://www.ros.org">
<ros2_control name="FullSpec" type="system">
<joint name="joint1">
<command_interface name="velocity">
<param name="min">-0.5</param>
<param name="max">0.5</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
<joint name="joint2">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
<transmission name="transmission1">
<plugin>transmission_interface/DifferentialTransmission</plugin>
<actuator name="joint1_motor" role="actuator1"/>
<actuator name="joint2_motor" role="actuator2"/>
<joint name="joint1" role="joint1"/>
<joint name="joint2" role="joint2"/>
</transmission>
</ros2_control>
</robot>
)";
std::vector<hardware_interface::HardwareInfo> infos =
hardware_interface::parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( infos[0].transmissions, SizeIs( 1 ) );
// Transmission loader
TransmissionPluginLoader loader;
std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
loader.create( infos[0].transmissions[0].type );
ASSERT_TRUE( nullptr != transmission_loader );
std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
transmission = transmission_loader->load( info );
// Validate transmission
transmission_interface::DifferentialTransmission * differential_transmission =
dynamic_cast<transmission_interface::DifferentialTransmission *>( transmission.get( ) );
const std::vector<double> & actuator_reduction =
differential_transmission->get_actuator_reduction( );
EXPECT_EQ( 1.0, actuator_reduction[0] );
EXPECT_EQ( 1.0, actuator_reduction[1] );
const std::vector<double> & joint_reduction = differential_transmission->get_joint_reduction( );
EXPECT_EQ( 1.0, joint_reduction[0] );
EXPECT_EQ( 1.0, joint_reduction[1] );
const std::vector<double> & joint_offset = differential_transmission->get_joint_offset( );
EXPECT_EQ( 0.0, joint_offset[0] );
EXPECT_EQ( 0.0, joint_offset[1] );
}
TEST( DifferentialTransmissionLoaderTest, mechanical_reduction_not_a_number )
{
// Parse transmission info
std::string urdf_to_test = R"(
<?xml version="1.0"?>
<robot name="robot" xmlns="http://www.ros.org">
<ros2_control name="FullSpec" type="system">
<joint name="joint1">
<command_interface name="velocity">
<param name="min">-0.5</param>
<param name="max">0.5</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
<joint name="joint2">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
<transmission name="transmission1">
<plugin>transmission_interface/DifferentialTransmission</plugin>
<actuator name="joint1_motor" role="actuator1">
<mechanical_reduction>one</mechanical_reduction>
</actuator>
<actuator name="joint2_motor" role="actuator2">
<mechanical_reduction>two</mechanical_reduction>
</actuator>
<joint name="joint1" role="joint1">
<mechanical_reduction>three</mechanical_reduction>
</joint>
<joint name="joint2" role="joint2">
<mechanical_reduction>four</mechanical_reduction>
</joint>
</transmission>
</ros2_control>
</robot>
)";
std::vector<hardware_interface::HardwareInfo> infos =
hardware_interface::parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( infos[0].transmissions, SizeIs( 1 ) );
// Transmission loader
TransmissionPluginLoader loader;
std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
loader.create( infos[0].transmissions[0].type );
ASSERT_TRUE( nullptr != transmission_loader );
std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
transmission = transmission_loader->load( info );
// Validate transmission
transmission_interface::DifferentialTransmission * differential_transmission =
dynamic_cast<transmission_interface::DifferentialTransmission *>( transmission.get( ) );
// default kicks in for ill-defined values
const std::vector<double> & actuator_reduction =
differential_transmission->get_actuator_reduction( );
EXPECT_EQ( 1.0, actuator_reduction[0] );
EXPECT_EQ( 1.0, actuator_reduction[1] );
const std::vector<double> & joint_reduction = differential_transmission->get_joint_reduction( );
EXPECT_EQ( 1.0, joint_reduction[0] );
EXPECT_EQ( 1.0, joint_reduction[1] );
const std::vector<double> & joint_offset = differential_transmission->get_joint_offset( );
EXPECT_EQ( 0.0, joint_offset[0] );
EXPECT_EQ( 0.0, joint_offset[1] );
}
TEST( DifferentialTransmissionLoaderTest, offset_ill_defined )
{
// Parse transmission info
std::string urdf_to_test = R"(
<?xml version="1.0"?>
<robot name="robot" xmlns="http://www.ros.org">
<ros2_control name="FullSpec" type="system">
<joint name="joint1">
<command_interface name="velocity">
<param name="min">-0.5</param>
<param name="max">0.5</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
<joint name="joint2">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
<transmission name="transmission1">
<plugin>transmission_interface/DifferentialTransmission</plugin>
<actuator name="joint1_motor" role="actuator1">
<mechanical_reduction>50</mechanical_reduction>
</actuator>
<actuator name="joint2_motor" role="actuator2">
<mechanical_reduction>-50</mechanical_reduction>
</actuator>
<joint name="joint1" role="joint1">
<offset>two</offset> <!-- Not a number -->
<mechanical_reduction>2.0</mechanical_reduction>
</joint>
<joint name="joint2" role="joint2">
<offset>three</offset> <!-- Not a number -->
<mechanical_reduction>-2.0</mechanical_reduction>
</joint>
</transmission>
</ros2_control>
</robot>
)";
std::vector<hardware_interface::HardwareInfo> infos =
hardware_interface::parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( infos[0].transmissions, SizeIs( 1 ) );
// Transmission loader
TransmissionPluginLoader loader;
std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
loader.create( infos[0].transmissions[0].type );
ASSERT_TRUE( nullptr != transmission_loader );
std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
transmission = transmission_loader->load( info );
// Validate transmission
transmission_interface::DifferentialTransmission * differential_transmission =
dynamic_cast<transmission_interface::DifferentialTransmission *>( transmission.get( ) );
// default kicks in for ill-defined values
const std::vector<double> & actuator_reduction =
differential_transmission->get_actuator_reduction( );
EXPECT_EQ( 50.0, actuator_reduction[0] );
EXPECT_EQ( -50.0, actuator_reduction[1] );
const std::vector<double> & joint_reduction = differential_transmission->get_joint_reduction( );
EXPECT_EQ( 2.0, joint_reduction[0] );
EXPECT_EQ( -2.0, joint_reduction[1] );
// default kicks in for ill-defined values
const std::vector<double> & joint_offset = differential_transmission->get_joint_offset( );
EXPECT_EQ( 0.0, joint_offset[0] );
EXPECT_EQ( 0.0, joint_offset[1] );
}
TEST( DifferentialTransmissionLoaderTest, mech_red_invalid_value )
{
// Parse transmission info
std::string urdf_to_test = R"(
<?xml version="1.0"?>
<robot name="robot" xmlns="http://www.ros.org">
<ros2_control name="FullSpec" type="system">
<joint name="joint1">
<command_interface name="velocity">
<param name="min">-0.5</param>
<param name="max">0.5</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
<joint name="joint2">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
<transmission name="transmission1">
<plugin>transmission_interface/DifferentialTransmission</plugin>
<actuator name="joint1_motor" role="actuator1">
<mechanical_reduction>0</mechanical_reduction>
</actuator>
<actuator name="joint2_motor" role="actuator2">
<mechanical_reduction>0</mechanical_reduction>
</actuator>
<joint name="joint1" role="joint1">
<offset>2</offset>
<mechanical_reduction>0</mechanical_reduction>
</joint>
<joint name="joint2" role="joint2">
<offset>3</offset>
<mechanical_reduction>0</mechanical_reduction>
</joint>
</transmission>
</ros2_control>
</robot>
)";
std::vector<hardware_interface::HardwareInfo> infos =
hardware_interface::parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( infos[0].transmissions, SizeIs( 1 ) );
// Transmission loader
TransmissionPluginLoader loader;
std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
loader.create( infos[0].transmissions[0].type );
ASSERT_TRUE( nullptr != transmission_loader );
std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
transmission = transmission_loader->load( info );
ASSERT_TRUE( nullptr == transmission );
}
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gmock/gmock.h>
#include <string>
#include <vector>
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "random_generator_utils.hpp"
#include "transmission_interface/differential_transmission.hpp"
using hardware_interface::HW_IF_EFFORT;
using hardware_interface::HW_IF_POSITION;
using hardware_interface::HW_IF_VELOCITY;
using testing::DoubleNear;
using transmission_interface::ActuatorHandle;
using transmission_interface::DifferentialTransmission;
using transmission_interface::Exception;
using transmission_interface::JointHandle;
// Floating-point value comparison threshold
const double EPS = 1e-6;
34 TEST( PreconditionsTest, ExceptionThrowing )
{
const std::vector<double> reduction_good = {1.0, 1.0};
const std::vector<double> reduction_bad1 = {0.0, 0.0};
const std::vector<double> reduction_bad2 = {1.0, 0.0};
const std::vector<double> reduction_bad3 = {0.0, 1.0};
const std::vector<double> offset_good = {1.0, 1.0};
// Invalid instance creation: Transmission cannot have zero reduction
EXPECT_THROW( DifferentialTransmission( reduction_bad1, reduction_good ), Exception );
EXPECT_THROW( DifferentialTransmission( reduction_bad2, reduction_good ), Exception );
EXPECT_THROW( DifferentialTransmission( reduction_bad3, reduction_good ), Exception );
EXPECT_THROW( DifferentialTransmission( reduction_good, reduction_bad1 ), Exception );
EXPECT_THROW( DifferentialTransmission( reduction_good, reduction_bad2 ), Exception );
EXPECT_THROW( DifferentialTransmission( reduction_good, reduction_bad3 ), Exception );
EXPECT_THROW( DifferentialTransmission( reduction_bad1, reduction_good, offset_good ), Exception );
EXPECT_THROW( DifferentialTransmission( reduction_bad2, reduction_good, offset_good ), Exception );
EXPECT_THROW( DifferentialTransmission( reduction_bad3, reduction_good, offset_good ), Exception );
EXPECT_THROW( DifferentialTransmission( reduction_good, reduction_bad1, offset_good ), Exception );
EXPECT_THROW( DifferentialTransmission( reduction_good, reduction_bad2, offset_good ), Exception );
EXPECT_THROW( DifferentialTransmission( reduction_good, reduction_bad3, offset_good ), Exception );
// Invalid instance creation: Wrong parameter sizes
const std::vector<double> reduction_bad_size = {1.0};
const std::vector<double> & offset_bad_size = reduction_bad_size;
EXPECT_THROW( DifferentialTransmission( reduction_bad_size, reduction_good ), Exception );
EXPECT_THROW( DifferentialTransmission( reduction_good, reduction_bad_size ), Exception );
EXPECT_THROW(
DifferentialTransmission( reduction_good, reduction_good, offset_bad_size ), Exception );
// Valid instance creation
EXPECT_NO_THROW( DifferentialTransmission( reduction_good, reduction_good ) );
EXPECT_NO_THROW( DifferentialTransmission( reduction_good, reduction_good, offset_good ) );
}
72 TEST( PreconditionsTest, AccessorValidation )
{
std::vector<double> act_reduction = {2.0, -2.0};
std::vector<double> jnt_reduction = {4.0, -4.0};
std::vector<double> jnt_offset = {1.0, -1.0};
DifferentialTransmission trans( act_reduction, jnt_reduction, jnt_offset );
EXPECT_EQ( 2u, trans.num_actuators( ) );
EXPECT_EQ( 2u, trans.num_joints( ) );
EXPECT_EQ( 2.0, trans.get_actuator_reduction( )[0] );
EXPECT_EQ( -2.0, trans.get_actuator_reduction( )[1] );
EXPECT_EQ( 4.0, trans.get_joint_reduction( )[0] );
EXPECT_EQ( -4.0, trans.get_joint_reduction( )[1] );
EXPECT_EQ( 1.0, trans.get_joint_offset( )[0] );
EXPECT_EQ( -1.0, trans.get_joint_offset( )[1] );
}
90 void testConfigureWithBadHandles( std::string interface_name )
{
DifferentialTransmission trans( {1.0, 1.0}, {1.0, 1.0} );
double dummy;
auto a1_handle = ActuatorHandle( "act1", interface_name, &dummy );
auto a2_handle = ActuatorHandle( "act2", interface_name, &dummy );
auto a3_handle = ActuatorHandle( "act3", interface_name, &dummy );
auto j1_handle = JointHandle( "joint1", interface_name, &dummy );
auto j2_handle = JointHandle( "joint2", interface_name, &dummy );
auto j3_handle = JointHandle( "joint3", interface_name, &dummy );
auto invalid_a1_handle = ActuatorHandle( "act1", interface_name, nullptr );
auto invalid_j1_handle = JointHandle( "joint1", interface_name, nullptr );
EXPECT_THROW( trans.configure( {}, {} ), Exception );
EXPECT_THROW( trans.configure( {j1_handle}, {} ), Exception );
EXPECT_THROW( trans.configure( {j1_handle}, {a1_handle} ), Exception );
EXPECT_THROW( trans.configure( {}, {a1_handle} ), Exception );
EXPECT_THROW( trans.configure( {j1_handle, j2_handle}, {a1_handle} ), Exception );
EXPECT_THROW( trans.configure( {j1_handle}, {a1_handle, a2_handle} ), Exception );
EXPECT_THROW(
trans.configure( {j1_handle, j2_handle, j3_handle}, {a1_handle, a2_handle} ), Exception );
EXPECT_THROW(
trans.configure( {j1_handle, j2_handle}, {a1_handle, a2_handle, a3_handle} ), Exception );
EXPECT_THROW(
trans.configure( {j1_handle, j2_handle, j3_handle}, {a1_handle, a2_handle, a3_handle} ),
Exception );
EXPECT_THROW( trans.configure( {j1_handle, j2_handle}, {invalid_a1_handle, a2_handle} ), Exception );
EXPECT_THROW( trans.configure( {invalid_j1_handle, j2_handle}, {a1_handle, a2_handle} ), Exception );
EXPECT_THROW(
trans.configure( {invalid_j1_handle, j2_handle}, {invalid_a1_handle, a2_handle} ), Exception );
}
123 TEST( ConfigureTest, FailsWithBadHandles )
{
testConfigureWithBadHandles( HW_IF_POSITION );
testConfigureWithBadHandles( HW_IF_VELOCITY );
testConfigureWithBadHandles( HW_IF_EFFORT );
}
130 class TransmissionSetup : public ::testing::Test
{
protected:
// Input/output transmission data
double a_val[2];
double j_val[2];
136 std::vector<double *> a_vec = {&a_val[0], &a_val[1]};
137 std::vector<double *> j_vec = {&j_val[0], &j_val[1]};
};
/// \brief Exercises the actuator->joint->actuator roundtrip, which should yield the identity map.
141 class BlackBoxTest : public TransmissionSetup
{
protected:
const double EXTREMAL_VALUE = 1337.1337;
/// \param trans Transmission instance.
/// \param ref_val Reference value that will be transformed with the respective forward
/// and inverse transmission transformations.
/// \param interface_name The name of the interface to test, position, velocity, etc.
149 void testIdentityMap(
150 DifferentialTransmission & trans, const std::vector<double> & ref_val,
151 const std::string & interface_name )
{
// set actuator values to reference
a_val[0] = ref_val[0];
a_val[1] = ref_val[1];
// create handles and configure
auto a1_handle = ActuatorHandle( "act1", interface_name, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", interface_name, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", interface_name, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", interface_name, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
// actuator->joint->actuator roundtrip
// but we also set actuator values to an extremal value
// to ensure joint_to_actuator is not a no-op
trans.actuator_to_joint( );
a_val[0] = a_val[1] = EXTREMAL_VALUE;
trans.joint_to_actuator( );
EXPECT_THAT( ref_val[0], DoubleNear( a_val[0], EPS ) );
EXPECT_THAT( ref_val[1], DoubleNear( a_val[1], EPS ) );
}
// Generate a set of transmission instances
// with random combinations of actuator/joint reduction and joint offset.
175 static std::vector<DifferentialTransmission> createTestInstances(
176 const vector<DifferentialTransmission>::size_type size )
{
std::vector<DifferentialTransmission> out;
out.reserve( size );
// NOTE: Magic value
RandomDoubleGenerator rand_gen( -1000.0, 1000.0 );
while ( out.size( ) < size )
{
try
{
DifferentialTransmission trans(
randomVector( 2, rand_gen ), randomVector( 2, rand_gen ), randomVector( 2, rand_gen ) );
out.push_back( trans );
}
catch ( const Exception & )
{
// NOTE: If by chance a perfect zero is produced by the random number generator,
// construction will fail
// We swallow the exception and move on to prevent a test crash.
}
}
return out;
}
};
202 TEST_F( BlackBoxTest, IdentityMap )
{
// Transmission instances
// NOTE: Magic value
auto transmission_test_instances = createTestInstances( 100 );
// Test different transmission configurations...
for ( auto && transmission : transmission_test_instances )
{
// ...and for each transmission, different input values
// NOTE: Magic value
RandomDoubleGenerator rand_gen( -1000.0, 1000.0 );
// NOTE: Magic value
const unsigned int input_value_trials = 100;
for ( unsigned int i = 0; i < input_value_trials; ++i )
{
vector<double> input_value = randomVector( 2, rand_gen );
// Test each interface type separately
testIdentityMap( transmission, input_value, HW_IF_POSITION );
testIdentityMap( transmission, input_value, HW_IF_VELOCITY );
testIdentityMap( transmission, input_value, HW_IF_EFFORT );
}
}
}
227 class WhiteBoxTest : public TransmissionSetup
{
};
231 TEST_F( WhiteBoxTest, DontMoveJoints )
{
std::vector<double> actuator_reduction = {10.0, 10.0};
std::vector<double> joint_reduction = {2.0, 2.0};
std::vector<double> joint_offset = {1.0, 1.0};
DifferentialTransmission trans( actuator_reduction, joint_reduction, joint_offset );
// Actuator input ( used for effort, velocity and position )
*a_vec[0] = 0.0;
*a_vec[1] = 0.0;
// Effort interface
{
auto a1_handle = ActuatorHandle( "act1", HW_IF_EFFORT, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", HW_IF_EFFORT, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", HW_IF_EFFORT, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", HW_IF_EFFORT, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
trans.actuator_to_joint( );
EXPECT_THAT( 0.0, DoubleNear( j_val[0], EPS ) );
EXPECT_THAT( 0.0, DoubleNear( j_val[1], EPS ) );
}
// Velocity interface
{
auto a1_handle = ActuatorHandle( "act1", HW_IF_VELOCITY, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", HW_IF_VELOCITY, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", HW_IF_VELOCITY, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", HW_IF_VELOCITY, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
trans.actuator_to_joint( );
EXPECT_THAT( 0.0, DoubleNear( j_val[0], EPS ) );
EXPECT_THAT( 0.0, DoubleNear( j_val[1], EPS ) );
}
// Position interface
{
auto a1_handle = ActuatorHandle( "act1", HW_IF_POSITION, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", HW_IF_POSITION, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", HW_IF_POSITION, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", HW_IF_POSITION, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
trans.actuator_to_joint( );
EXPECT_THAT( joint_offset[0], DoubleNear( j_val[0], EPS ) );
EXPECT_THAT( joint_offset[1], DoubleNear( j_val[1], EPS ) );
}
}
280 TEST_F( WhiteBoxTest, MoveFirstJointOnly )
{
std::vector<double> actuator_reduction = {10.0, 10.0};
std::vector<double> joint_reduction = {2.0, 2.0};
DifferentialTransmission trans( actuator_reduction, joint_reduction );
// Actuator input ( used for effort, velocity and position )
*a_vec[0] = 10.0;
*a_vec[1] = 10.0;
// Effort interface
{
auto a1_handle = ActuatorHandle( "act1", HW_IF_EFFORT, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", HW_IF_EFFORT, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", HW_IF_EFFORT, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", HW_IF_EFFORT, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
trans.actuator_to_joint( );
EXPECT_THAT( 400.0, DoubleNear( j_val[0], EPS ) );
EXPECT_THAT( 0.0, DoubleNear( j_val[1], EPS ) );
}
// Velocity interface
{
auto a1_handle = ActuatorHandle( "act1", HW_IF_VELOCITY, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", HW_IF_VELOCITY, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", HW_IF_VELOCITY, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", HW_IF_VELOCITY, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
trans.actuator_to_joint( );
EXPECT_THAT( 0.5, DoubleNear( j_val[0], EPS ) );
EXPECT_THAT( 0.0, DoubleNear( j_val[1], EPS ) );
}
// Position interface
{
auto a1_handle = ActuatorHandle( "act1", HW_IF_POSITION, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", HW_IF_POSITION, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", HW_IF_POSITION, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", HW_IF_POSITION, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
trans.actuator_to_joint( );
EXPECT_THAT( 0.5, DoubleNear( j_val[0], EPS ) );
EXPECT_THAT( 0.0, DoubleNear( j_val[1], EPS ) );
}
}
328 TEST_F( WhiteBoxTest, MoveSecondJointOnly )
{
std::vector<double> actuator_reduction = {10.0, 10.0};
std::vector<double> joint_reduction = {2.0, 2.0};
DifferentialTransmission trans( actuator_reduction, joint_reduction );
// Actuator input ( used for effort, velocity and position )
*a_vec[0] = 10.0;
*a_vec[1] = -10.0;
// Effort interface
{
auto a1_handle = ActuatorHandle( "act1", HW_IF_EFFORT, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", HW_IF_EFFORT, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", HW_IF_EFFORT, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", HW_IF_EFFORT, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
trans.actuator_to_joint( );
EXPECT_THAT( 0.0, DoubleNear( j_val[0], EPS ) );
EXPECT_THAT( 400.0, DoubleNear( j_val[1], EPS ) );
}
// Velocity interface
{
auto a1_handle = ActuatorHandle( "act1", HW_IF_VELOCITY, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", HW_IF_VELOCITY, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", HW_IF_VELOCITY, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", HW_IF_VELOCITY, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
trans.actuator_to_joint( );
EXPECT_THAT( 0.0, DoubleNear( j_val[0], EPS ) );
EXPECT_THAT( 0.5, DoubleNear( j_val[1], EPS ) );
}
// Position interface
{
auto a1_handle = ActuatorHandle( "act1", HW_IF_POSITION, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", HW_IF_POSITION, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", HW_IF_POSITION, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", HW_IF_POSITION, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
trans.actuator_to_joint( );
EXPECT_THAT( 0.0, DoubleNear( j_val[0], EPS ) );
EXPECT_THAT( 0.5, DoubleNear( j_val[1], EPS ) );
}
}
376 TEST_F( WhiteBoxTest, MoveBothJoints )
{
// NOTE: We only test the actuator->joint map,
// as the joint->actuator map is indirectly validated in the test that
// checks that actuator->joint->actuator == identity.
std::vector<double> actuator_reduction = {10.0, -20.0};
std::vector<double> joint_reduction = {-2.0, 4.0};
std::vector<double> joint_offset = {-2.0, 4.0};
DifferentialTransmission trans( actuator_reduction, joint_reduction, joint_offset );
// Actuator input ( used for effort, velocity and position )
*a_vec[0] = 3.0;
*a_vec[1] = 5.0;
// Effort interface
{
auto a1_handle = ActuatorHandle( "act1", HW_IF_EFFORT, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", HW_IF_EFFORT, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", HW_IF_EFFORT, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", HW_IF_EFFORT, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
trans.actuator_to_joint( );
EXPECT_THAT( 140.0, DoubleNear( j_val[0], EPS ) );
EXPECT_THAT( 520.0, DoubleNear( j_val[1], EPS ) );
}
// Velocity interface
{
auto a1_handle = ActuatorHandle( "act1", HW_IF_VELOCITY, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", HW_IF_VELOCITY, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", HW_IF_VELOCITY, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", HW_IF_VELOCITY, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
trans.actuator_to_joint( );
EXPECT_THAT( -0.01250, DoubleNear( j_val[0], EPS ) );
EXPECT_THAT( 0.06875, DoubleNear( j_val[1], EPS ) );
}
// Position interface
{
auto a1_handle = ActuatorHandle( "act1", HW_IF_POSITION, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", HW_IF_POSITION, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", HW_IF_POSITION, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", HW_IF_POSITION, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
trans.actuator_to_joint( );
EXPECT_THAT( -2.01250, DoubleNear( j_val[0], EPS ) );
EXPECT_THAT( 4.06875, DoubleNear( j_val[1], EPS ) );
}
}
1 // Copyright 2022 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gmock/gmock.h>
#include <exception>
#include <memory>
#include <string>
#include <vector>
#include "hardware_interface/component_parser.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "pluginlib/class_loader.hpp"
#include "transmission_interface/four_bar_linkage_transmission.hpp"
#include "transmission_interface/four_bar_linkage_transmission_loader.hpp"
#include "transmission_interface/transmission_loader.hpp"
using testing::SizeIs;
31 class TransmissionPluginLoader
{
public:
34 std::shared_ptr<transmission_interface::TransmissionLoader> create( const std::string & type )
{
try
{
return class_loader_.createUniqueInstance( type );
}
catch ( std::exception & ex )
{
std::cerr << ex.what( ) << std::endl;
return std::shared_ptr<transmission_interface::TransmissionLoader>( );
}
}
private:
// must keep it alive because instance destroyers need it
49 pluginlib::ClassLoader<transmission_interface::TransmissionLoader> class_loader_ = {
"transmission_interface", "transmission_interface::TransmissionLoader"};
};
53 TEST( FourBarLinkageTransmissionLoaderTest, FullSpec )
{
// Parse transmission info
std::string urdf_to_test = R"(
<?xml version="1.0"?>
<robot name="robot" xmlns="http://www.ros.org">
<ros2_control name="FullSpec" type="system">
<joint name="joint1">
<command_interface name="velocity">
<param name="min">-0.5</param>
<param name="max">0.5</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
<joint name="joint2">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
<transmission name="transmission1">
<plugin>transmission_interface/FourBarLinkageTransmission</plugin>
<actuator name="joint1_motor" role="actuator1">
<mechanical_reduction>50</mechanical_reduction>
</actuator>
<actuator name="joint2_motor" role="actuator2">
<mechanical_reduction>-50</mechanical_reduction>
</actuator>
<joint name="joint1" role="joint1">
<offset>0.5</offset>
<mechanical_reduction>2.0</mechanical_reduction>
</joint>
<joint name="joint2" role="joint2">
<offset>-0.5</offset>
<mechanical_reduction>-2.0</mechanical_reduction>
</joint>
</transmission>
</ros2_control>
</robot>
)";
std::vector<hardware_interface::HardwareInfo> infos =
hardware_interface::parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( infos[0].transmissions, SizeIs( 1 ) );
// Transmission loader
TransmissionPluginLoader loader;
std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
loader.create( infos[0].transmissions[0].type );
ASSERT_TRUE( nullptr != transmission_loader );
std::shared_ptr<transmission_interface::Transmission> transmission;
const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
transmission = transmission_loader->load( info );
// Validate transmission
transmission_interface::FourBarLinkageTransmission * four_bar_linkage_transmission =
dynamic_cast<transmission_interface::FourBarLinkageTransmission *>( transmission.get( ) );
ASSERT_TRUE( nullptr != four_bar_linkage_transmission );
const std::vector<double> & actuator_reduction =
four_bar_linkage_transmission->get_actuator_reduction( );
EXPECT_EQ( 50.0, actuator_reduction[0] );
EXPECT_EQ( -50.0, actuator_reduction[1] );
const std::vector<double> & joint_reduction =
four_bar_linkage_transmission->get_joint_reduction( );
EXPECT_EQ( 2.0, joint_reduction[0] );
EXPECT_EQ( -2.0, joint_reduction[1] );
const std::vector<double> & joint_offset = four_bar_linkage_transmission->get_joint_offset( );
EXPECT_EQ( 0.5, joint_offset[0] );
EXPECT_EQ( -0.5, joint_offset[1] );
}
TEST( FourBarLinkageTransmissionLoaderTest, only_mech_red_specified )
{
// Parse transmission info
std::string urdf_to_test = R"(
<?xml version="1.0"?>
<robot name="robot" xmlns="http://www.ros.org">
<ros2_control name="FullSpec" type="system">
<joint name="joint1">
<command_interface name="velocity">
<param name="min">-0.5</param>
<param name="max">0.5</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
<joint name="joint2">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
<transmission name="transmission1">
<plugin>transmission_interface/FourBarLinkageTransmission</plugin>
<actuator name="joint1_motor" role="actuator1">
<mechanical_reduction>50</mechanical_reduction>
</actuator>
<actuator name="joint2_motor" role="actuator2">
<mechanical_reduction>-50</mechanical_reduction>
</actuator>
<joint name="joint1" role="joint1">
<mechanical_reduction>1.0</mechanical_reduction>
</joint>
<joint name="joint2" role="joint2">
<mechanical_reduction>1.0</mechanical_reduction>
</joint>
</transmission>
</ros2_control>
</robot>
)";
std::vector<hardware_interface::HardwareInfo> infos =
hardware_interface::parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( infos[0].transmissions, SizeIs( 1 ) );
// Transmission loader
TransmissionPluginLoader loader;
std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
loader.create( infos[0].transmissions[0].type );
ASSERT_TRUE( nullptr != transmission_loader );
std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
transmission = transmission_loader->load( info );
// Validate transmission
transmission_interface::FourBarLinkageTransmission * four_bar_linkage_transmission =
dynamic_cast<transmission_interface::FourBarLinkageTransmission *>( transmission.get( ) );
const std::vector<double> & actuator_reduction =
four_bar_linkage_transmission->get_actuator_reduction( );
EXPECT_EQ( 50.0, actuator_reduction[0] );
EXPECT_EQ( -50.0, actuator_reduction[1] );
const std::vector<double> & joint_reduction =
four_bar_linkage_transmission->get_joint_reduction( );
EXPECT_EQ( 1.0, joint_reduction[0] );
EXPECT_EQ( 1.0, joint_reduction[1] );
const std::vector<double> & joint_offset = four_bar_linkage_transmission->get_joint_offset( );
EXPECT_EQ( 0.0, joint_offset[0] );
EXPECT_EQ( 0.0, joint_offset[1] );
}
TEST( DifferentialTransmissionLoaderTest, offset_and_mech_red_not_specified )
{
// Parse transmission info
std::string urdf_to_test = R"(
<?xml version="1.0"?>
<robot name="robot" xmlns="http://www.ros.org">
<ros2_control name="FullSpec" type="system">
<joint name="joint1">
<command_interface name="velocity">
<param name="min">-0.5</param>
<param name="max">0.5</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
<joint name="joint2">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
<transmission name="transmission1">
<plugin>transmission_interface/FourBarLinkageTransmission</plugin>
<actuator name="joint1_motor" role="actuator1"/>
<actuator name="joint2_motor" role="actuator2"/>
<joint name="joint1" role="joint1"/>
<joint name="joint2" role="joint2"/>
</transmission>
</ros2_control>
</robot>
)";
std::vector<hardware_interface::HardwareInfo> infos =
hardware_interface::parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( infos[0].transmissions, SizeIs( 1 ) );
// Transmission loader
TransmissionPluginLoader loader;
std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
loader.create( infos[0].transmissions[0].type );
ASSERT_TRUE( nullptr != transmission_loader );
std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
transmission = transmission_loader->load( info );
// Validate transmission
transmission_interface::FourBarLinkageTransmission * four_bar_linkage_transmission =
dynamic_cast<transmission_interface::FourBarLinkageTransmission *>( transmission.get( ) );
const std::vector<double> & actuator_reduction =
four_bar_linkage_transmission->get_actuator_reduction( );
EXPECT_EQ( 1.0, actuator_reduction[0] );
EXPECT_EQ( 1.0, actuator_reduction[1] );
const std::vector<double> & joint_reduction =
four_bar_linkage_transmission->get_joint_reduction( );
EXPECT_EQ( 1.0, joint_reduction[0] );
EXPECT_EQ( 1.0, joint_reduction[1] );
const std::vector<double> & joint_offset = four_bar_linkage_transmission->get_joint_offset( );
EXPECT_EQ( 0.0, joint_offset[0] );
EXPECT_EQ( 0.0, joint_offset[1] );
}
TEST( FourBarLinkageTransmissionLoaderTest, mechanical_reduction_not_a_number )
{
// Parse transmission info
std::string urdf_to_test = R"(
<?xml version="1.0"?>
<robot name="robot" xmlns="http://www.ros.org">
<ros2_control name="FullSpec" type="system">
<joint name="joint1">
<command_interface name="velocity">
<param name="min">-0.5</param>
<param name="max">0.5</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
<joint name="joint2">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
<transmission name="transmission1">
<plugin>transmission_interface/FourBarLinkageTransmission</plugin>
<actuator name="joint1_motor" role="actuator1">
<mechanical_reduction>one</mechanical_reduction>
</actuator>
<actuator name="joint2_motor" role="actuator2">
<mechanical_reduction>two</mechanical_reduction>
</actuator>
<joint name="joint1" role="joint1">
<mechanical_reduction>three</mechanical_reduction>
</joint>
<joint name="joint2" role="joint2">
<mechanical_reduction>four</mechanical_reduction>
</joint>
</transmission>
</ros2_control>
</robot>
)";
std::vector<hardware_interface::HardwareInfo> infos =
hardware_interface::parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( infos[0].transmissions, SizeIs( 1 ) );
// Transmission loader
TransmissionPluginLoader loader;
std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
loader.create( infos[0].transmissions[0].type );
ASSERT_TRUE( nullptr != transmission_loader );
std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
transmission = transmission_loader->load( info );
// Validate transmission
transmission_interface::FourBarLinkageTransmission * four_bar_linkage_transmission =
dynamic_cast<transmission_interface::FourBarLinkageTransmission *>( transmission.get( ) );
// default kicks in for ill-defined values
const std::vector<double> & actuator_reduction =
four_bar_linkage_transmission->get_actuator_reduction( );
EXPECT_EQ( 1.0, actuator_reduction[0] );
EXPECT_EQ( 1.0, actuator_reduction[1] );
const std::vector<double> & joint_reduction =
four_bar_linkage_transmission->get_joint_reduction( );
EXPECT_EQ( 1.0, joint_reduction[0] );
EXPECT_EQ( 1.0, joint_reduction[1] );
const std::vector<double> & joint_offset = four_bar_linkage_transmission->get_joint_offset( );
EXPECT_EQ( 0.0, joint_offset[0] );
EXPECT_EQ( 0.0, joint_offset[1] );
}
TEST( FourBarLinkageTransmissionLoaderTest, offset_ill_defined )
{
// Parse transmission info
std::string urdf_to_test = R"(
<?xml version="1.0"?>
<robot name="robot" xmlns="http://www.ros.org">
<ros2_control name="FullSpec" type="system">
<joint name="joint1">
<command_interface name="velocity">
<param name="min">-0.5</param>
<param name="max">0.5</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
<joint name="joint2">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
<transmission name="transmission1">
<plugin>transmission_interface/FourBarLinkageTransmission</plugin>
<actuator name="joint1_motor" role="actuator1">
<mechanical_reduction>50</mechanical_reduction>
</actuator>
<actuator name="joint2_motor" role="actuator2">
<mechanical_reduction>-50</mechanical_reduction>
</actuator>
<joint name="joint1" role="joint1">
<offset>two</offset> <!-- Not a number -->
<mechanical_reduction>2.0</mechanical_reduction>
</joint>
<joint name="joint2" role="joint2">
<offset>three</offset> <!-- Not a number -->
<mechanical_reduction>-2.0</mechanical_reduction>
</joint>
</transmission>
</ros2_control>
</robot>
)";
std::vector<hardware_interface::HardwareInfo> infos =
hardware_interface::parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( infos[0].transmissions, SizeIs( 1 ) );
// Transmission loader
TransmissionPluginLoader loader;
std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
loader.create( infos[0].transmissions[0].type );
ASSERT_TRUE( nullptr != transmission_loader );
std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
transmission = transmission_loader->load( info );
// Validate transmission
transmission_interface::FourBarLinkageTransmission * four_bar_linkage_transmission =
dynamic_cast<transmission_interface::FourBarLinkageTransmission *>( transmission.get( ) );
// default kicks in for ill-defined values
const std::vector<double> & actuator_reduction =
four_bar_linkage_transmission->get_actuator_reduction( );
EXPECT_EQ( 50.0, actuator_reduction[0] );
EXPECT_EQ( -50.0, actuator_reduction[1] );
const std::vector<double> & joint_reduction =
four_bar_linkage_transmission->get_joint_reduction( );
EXPECT_EQ( 2.0, joint_reduction[0] );
EXPECT_EQ( -2.0, joint_reduction[1] );
// default kicks in for ill-defined values
const std::vector<double> & joint_offset = four_bar_linkage_transmission->get_joint_offset( );
EXPECT_EQ( 0.0, joint_offset[0] );
EXPECT_EQ( 0.0, joint_offset[1] );
}
TEST( FourBarLinkageTransmissionLoaderTest, mech_red_invalid_value )
{
// Parse transmission info
std::string urdf_to_test = R"(
<?xml version="1.0"?>
<robot name="robot" xmlns="http://www.ros.org">
<ros2_control name="FullSpec" type="system">
<joint name="joint1">
<command_interface name="velocity">
<param name="min">-0.5</param>
<param name="max">0.5</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
<joint name="joint2">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
<transmission name="transmission1">
<plugin>transmission_interface/FourBarLinkageTransmission</plugin>
<actuator name="joint1_motor" role="actuator1">
<mechanical_reduction>0</mechanical_reduction>
</actuator>
<actuator name="joint2_motor" role="actuator2">
<mechanical_reduction>0</mechanical_reduction>
</actuator>
<joint name="joint1" role="joint1">
<offset>2</offset>
<mechanical_reduction>0</mechanical_reduction>
</joint>
<joint name="joint2" role="joint2">
<offset>3</offset>
<mechanical_reduction>0</mechanical_reduction>
</joint>
</transmission>
</ros2_control>
</robot>
)";
std::vector<hardware_interface::HardwareInfo> infos =
hardware_interface::parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( infos[0].transmissions, SizeIs( 1 ) );
// Transmission loader
TransmissionPluginLoader loader;
std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
loader.create( infos[0].transmissions[0].type );
ASSERT_TRUE( nullptr != transmission_loader );
std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
transmission = transmission_loader->load( info );
ASSERT_TRUE( nullptr == transmission );
}
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/// \author Adolfo Rodriguez Tsouroukdissian
#include <gmock/gmock.h>
#include <string>
#include <vector>
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "random_generator_utils.hpp"
#include "transmission_interface/four_bar_linkage_transmission.hpp"
using hardware_interface::HW_IF_EFFORT;
using hardware_interface::HW_IF_POSITION;
using hardware_interface::HW_IF_VELOCITY;
using testing::DoubleNear;
using testing::Not;
using transmission_interface::ActuatorHandle;
using transmission_interface::Exception;
using transmission_interface::FourBarLinkageTransmission;
using transmission_interface::JointHandle;
// Floating-point value comparison threshold
const double EPS = 1e-6;
38 TEST( PreconditionsTest, ExceptionThrowing )
{
const std::vector<double> reduction_good = {1.0, 1.0};
const std::vector<double> reduction_bad1 = {0.0, 0.0};
const std::vector<double> reduction_bad2 = {1.0, 0.0};
const std::vector<double> reduction_bad3 = {0.0, 1.0};
const std::vector<double> offset_good = {1.0, 1.0};
// Invalid instance creation: Transmission cannot have zero reduction
EXPECT_THROW( FourBarLinkageTransmission( reduction_bad1, reduction_good ), Exception );
EXPECT_THROW( FourBarLinkageTransmission( reduction_bad2, reduction_good ), Exception );
EXPECT_THROW( FourBarLinkageTransmission( reduction_bad3, reduction_good ), Exception );
EXPECT_THROW( FourBarLinkageTransmission( reduction_good, reduction_bad1 ), Exception );
EXPECT_THROW( FourBarLinkageTransmission( reduction_good, reduction_bad2 ), Exception );
EXPECT_THROW( FourBarLinkageTransmission( reduction_good, reduction_bad3 ), Exception );
EXPECT_THROW( FourBarLinkageTransmission( reduction_bad1, reduction_good, offset_good ), Exception );
EXPECT_THROW( FourBarLinkageTransmission( reduction_bad2, reduction_good, offset_good ), Exception );
EXPECT_THROW( FourBarLinkageTransmission( reduction_bad3, reduction_good, offset_good ), Exception );
EXPECT_THROW( FourBarLinkageTransmission( reduction_good, reduction_bad1, offset_good ), Exception );
EXPECT_THROW( FourBarLinkageTransmission( reduction_good, reduction_bad2, offset_good ), Exception );
EXPECT_THROW( FourBarLinkageTransmission( reduction_good, reduction_bad3, offset_good ), Exception );
// Invalid instance creation: Wrong parameter sizes
const std::vector<double> reduction_bad_size = {1.0};
const std::vector<double> & offset_bad_size = reduction_bad_size;
EXPECT_THROW( FourBarLinkageTransmission( reduction_bad_size, reduction_good ), Exception );
EXPECT_THROW( FourBarLinkageTransmission( reduction_good, reduction_bad_size ), Exception );
EXPECT_THROW(
FourBarLinkageTransmission( reduction_good, reduction_good, offset_bad_size ), Exception );
// Valid instance creation
EXPECT_NO_THROW( FourBarLinkageTransmission( reduction_good, reduction_good ) );
EXPECT_NO_THROW( FourBarLinkageTransmission( reduction_good, reduction_good, offset_good ) );
}
76 TEST( PreconditionsTest, AccessorValidation )
{
std::vector<double> act_reduction = {2.0, -2.0};
std::vector<double> jnt_reduction = {4.0, -4.0};
std::vector<double> jnt_offset = {1.0, -1.0};
FourBarLinkageTransmission trans( act_reduction, jnt_reduction, jnt_offset );
EXPECT_EQ( 2u, trans.num_actuators( ) );
EXPECT_EQ( 2u, trans.num_joints( ) );
EXPECT_EQ( 2.0, trans.get_actuator_reduction( )[0] );
EXPECT_EQ( -2.0, trans.get_actuator_reduction( )[1] );
EXPECT_EQ( 4.0, trans.get_joint_reduction( )[0] );
EXPECT_EQ( -4.0, trans.get_joint_reduction( )[1] );
EXPECT_EQ( 1.0, trans.get_joint_offset( )[0] );
EXPECT_EQ( -1.0, trans.get_joint_offset( )[1] );
}
94 void testConfigureWithBadHandles( std::string interface_name )
{
FourBarLinkageTransmission trans( {1.0, 1.0}, {1.0, 1.0} );
double dummy;
auto a1_handle = ActuatorHandle( "act1", interface_name, &dummy );
auto a2_handle = ActuatorHandle( "act2", interface_name, &dummy );
auto a3_handle = ActuatorHandle( "act3", interface_name, &dummy );
auto j1_handle = JointHandle( "joint1", interface_name, &dummy );
auto j2_handle = JointHandle( "joint2", interface_name, &dummy );
auto j3_handle = JointHandle( "joint3", interface_name, &dummy );
auto invalid_a1_handle = ActuatorHandle( "act1", interface_name, nullptr );
auto invalid_j1_handle = JointHandle( "joint1", interface_name, nullptr );
EXPECT_THROW( trans.configure( {}, {} ), Exception );
EXPECT_THROW( trans.configure( {j1_handle}, {} ), Exception );
EXPECT_THROW( trans.configure( {j1_handle}, {a1_handle} ), Exception );
EXPECT_THROW( trans.configure( {}, {a1_handle} ), Exception );
EXPECT_THROW( trans.configure( {j1_handle, j2_handle}, {a1_handle} ), Exception );
EXPECT_THROW( trans.configure( {j1_handle}, {a1_handle, a2_handle} ), Exception );
EXPECT_THROW(
trans.configure( {j1_handle, j2_handle, j3_handle}, {a1_handle, a2_handle} ), Exception );
EXPECT_THROW(
trans.configure( {j1_handle, j2_handle}, {a1_handle, a2_handle, a3_handle} ), Exception );
EXPECT_THROW(
trans.configure( {j1_handle, j2_handle, j3_handle}, {a1_handle, a2_handle, a3_handle} ),
Exception );
EXPECT_THROW( trans.configure( {j1_handle, j2_handle}, {invalid_a1_handle, a2_handle} ), Exception );
EXPECT_THROW( trans.configure( {invalid_j1_handle, j2_handle}, {a1_handle, a2_handle} ), Exception );
EXPECT_THROW(
trans.configure( {invalid_j1_handle, j2_handle}, {invalid_a1_handle, a2_handle} ), Exception );
}
127 TEST( ConfigureTest, FailsWithBadHandles )
{
testConfigureWithBadHandles( HW_IF_POSITION );
testConfigureWithBadHandles( HW_IF_VELOCITY );
testConfigureWithBadHandles( HW_IF_EFFORT );
}
134 class TransmissionSetup : public ::testing::Test
{
protected:
// Input/output transmission data
double a_val[2];
double j_val[2];
140 std::vector<double *> a_vec = {&a_val[0], &a_val[1]};
141 std::vector<double *> j_vec = {&j_val[0], &j_val[1]};
};
/// \brief Exercises the actuator->joint->actuator roundtrip, which should yield the identity map.
145 class BlackBoxTest : public TransmissionSetup
{
protected:
const double EXTREMAL_VALUE = 1337.1337;
/// \param trans Transmission instance.
/// \param ref_val Reference value that will be transformed with the respective forward
/// and inverse transmission transformations.
/// \param interface_name The name of the interface to test, position, velocity, etc.
153 void testIdentityMap(
154 FourBarLinkageTransmission & trans, const std::vector<double> & ref_val,
155 const std::string & interface_name )
{
// set actuator values to reference
a_val[0] = ref_val[0];
a_val[1] = ref_val[1];
// create handles and configure
auto a1_handle = ActuatorHandle( "act1", interface_name, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", interface_name, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", interface_name, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", interface_name, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
// actuator->joint->actuator roundtrip
// but we also set actuator values to an extremal value
// to ensure joint_to_actuator is not a no-op
trans.actuator_to_joint( );
a_val[0] = a_val[1] = EXTREMAL_VALUE;
trans.joint_to_actuator( );
EXPECT_THAT( ref_val[0], DoubleNear( a_val[0], EPS ) );
EXPECT_THAT( ref_val[1], DoubleNear( a_val[1], EPS ) );
}
// Generate a set of transmission instances
// with random combinations of actuator/joint reduction and joint offset.
179 static std::vector<FourBarLinkageTransmission> createTestInstances(
180 const vector<FourBarLinkageTransmission>::size_type size )
{
std::vector<FourBarLinkageTransmission> out;
out.reserve( size );
// NOTE: Magic value
RandomDoubleGenerator rand_gen( -1000.0, 1000.0 );
while ( out.size( ) < size )
{
try
{
FourBarLinkageTransmission trans(
randomVector( 2, rand_gen ), randomVector( 2, rand_gen ), randomVector( 2, rand_gen ) );
out.push_back( trans );
}
catch ( const Exception & )
{
// NOTE: If by chance a perfect zero is produced by the random number generator,
// construction will fail
// We swallow the exception and move on to prevent a test crash.
}
}
return out;
}
};
206 TEST_F( BlackBoxTest, IdentityMap )
{
// Transmission instances
// NOTE: Magic value
auto transmission_test_instances = createTestInstances( 100 );
// Test different transmission configurations...
for ( auto && transmission : transmission_test_instances )
{
// ...and for each transmission, different input values
// NOTE: Magic value
RandomDoubleGenerator rand_gen( -1000.0, 1000.0 );
// NOTE: Magic value
const unsigned int input_value_trials = 100;
for ( unsigned int i = 0; i < input_value_trials; ++i )
{
vector<double> input_value = randomVector( 2, rand_gen );
// Test each interface type separately
testIdentityMap( transmission, input_value, HW_IF_POSITION );
testIdentityMap( transmission, input_value, HW_IF_VELOCITY );
testIdentityMap( transmission, input_value, HW_IF_EFFORT );
}
}
}
231 class WhiteBoxTest : public TransmissionSetup
{
};
235 TEST_F( WhiteBoxTest, DontMoveJoints )
{
std::vector<double> actuator_reduction = {10.0, 10.0};
std::vector<double> joint_reduction = {2.0, 2.0};
std::vector<double> joint_offset = {1.0, 1.0};
FourBarLinkageTransmission trans( actuator_reduction, joint_reduction, joint_offset );
// Actuator input ( used for effort, velocity and position )
a_val[0] = 0.0;
a_val[1] = 0.0;
// Effort interface
{
auto a1_handle = ActuatorHandle( "act1", HW_IF_EFFORT, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", HW_IF_EFFORT, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", HW_IF_EFFORT, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", HW_IF_EFFORT, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
trans.actuator_to_joint( );
EXPECT_THAT( 0.0, DoubleNear( j_val[0], EPS ) );
EXPECT_THAT( 0.0, DoubleNear( j_val[1], EPS ) );
}
// Velocity interface
{
auto a1_handle = ActuatorHandle( "act1", HW_IF_VELOCITY, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", HW_IF_VELOCITY, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", HW_IF_VELOCITY, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", HW_IF_VELOCITY, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
trans.actuator_to_joint( );
EXPECT_THAT( 0.0, DoubleNear( j_val[0], EPS ) );
EXPECT_THAT( 0.0, DoubleNear( j_val[1], EPS ) );
}
// Position interface
{
auto a1_handle = ActuatorHandle( "act1", HW_IF_POSITION, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", HW_IF_POSITION, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", HW_IF_POSITION, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", HW_IF_POSITION, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
trans.actuator_to_joint( );
EXPECT_THAT( joint_offset[0], DoubleNear( j_val[0], EPS ) );
EXPECT_THAT( joint_offset[1], DoubleNear( j_val[1], EPS ) );
}
}
284 TEST_F( WhiteBoxTest, MoveFirstJointOnly )
{
std::vector<double> actuator_reduction = {10.0, 10.0};
std::vector<double> joint_reduction = {2.0, 2.0};
FourBarLinkageTransmission trans( actuator_reduction, joint_reduction );
// Effort interface
{
a_val[0] = 5.0;
a_val[1] = 10.0;
auto a1_handle = ActuatorHandle( "act1", HW_IF_EFFORT, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", HW_IF_EFFORT, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", HW_IF_EFFORT, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", HW_IF_EFFORT, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
trans.actuator_to_joint( );
EXPECT_THAT( 100.0, DoubleNear( j_val[0], EPS ) );
EXPECT_THAT( 0.0, DoubleNear( j_val[1], EPS ) );
}
// Velocity interface
{
a_val[0] = 10.0;
a_val[1] = 5.0;
auto a1_handle = ActuatorHandle( "act1", HW_IF_VELOCITY, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", HW_IF_VELOCITY, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", HW_IF_VELOCITY, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", HW_IF_VELOCITY, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
trans.actuator_to_joint( );
EXPECT_THAT( 0.5, DoubleNear( j_val[0], EPS ) );
EXPECT_THAT( 0.0, DoubleNear( j_val[1], EPS ) );
}
// Position interface
{
a_val[0] = 10.0;
a_val[1] = 5.0;
auto a1_handle = ActuatorHandle( "act1", HW_IF_POSITION, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", HW_IF_POSITION, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", HW_IF_POSITION, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", HW_IF_POSITION, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
trans.actuator_to_joint( );
EXPECT_THAT( 0.5, DoubleNear( j_val[0], EPS ) );
EXPECT_THAT( 0.0, DoubleNear( j_val[1], EPS ) );
}
}
334 TEST_F( WhiteBoxTest, MoveSecondJointOnly )
{
std::vector<double> actuator_reduction = {10.0, 10.0};
std::vector<double> joint_reduction = {2.0, 2.0};
FourBarLinkageTransmission trans( actuator_reduction, joint_reduction );
// Actuator input ( used for effort, velocity and position )
a_val[0] = 0.0;
a_val[1] = 10.0;
// Effort interface
{
auto a1_handle = ActuatorHandle( "act1", HW_IF_EFFORT, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", HW_IF_EFFORT, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", HW_IF_EFFORT, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", HW_IF_EFFORT, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
trans.actuator_to_joint( );
EXPECT_THAT( 0.0, DoubleNear( j_val[0], EPS ) );
EXPECT_THAT( 200.0, DoubleNear( j_val[1], EPS ) );
}
// Velocity interface
{
auto a1_handle = ActuatorHandle( "act1", HW_IF_VELOCITY, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", HW_IF_VELOCITY, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", HW_IF_VELOCITY, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", HW_IF_VELOCITY, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
trans.actuator_to_joint( );
EXPECT_THAT( 0.0, DoubleNear( j_val[0], EPS ) );
EXPECT_THAT( 0.5, DoubleNear( j_val[1], EPS ) );
}
// Position interface
{
auto a1_handle = ActuatorHandle( "act1", HW_IF_POSITION, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", HW_IF_POSITION, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", HW_IF_POSITION, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", HW_IF_POSITION, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
trans.actuator_to_joint( );
EXPECT_THAT( 0.0, DoubleNear( j_val[0], EPS ) );
EXPECT_THAT( 0.5, DoubleNear( j_val[1], EPS ) );
}
}
382 TEST_F( WhiteBoxTest, MoveBothJoints )
{
// NOTE: We only test the actuator->joint map,
// as the joint->actuator map is indirectly validated in the test that
// checks that actuator->joint->actuator == identity.
std::vector<double> actuator_reduction = {10.0, -20.0};
std::vector<double> joint_reduction = {-2.0, 4.0};
std::vector<double> joint_offset = {-2.0, 4.0};
FourBarLinkageTransmission trans( actuator_reduction, joint_reduction, joint_offset );
// Actuator input ( used for effort, velocity and position )
a_val[0] = 3.0;
a_val[1] = 5.0;
// Effort interface
{
auto a1_handle = ActuatorHandle( "act1", HW_IF_EFFORT, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", HW_IF_EFFORT, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", HW_IF_EFFORT, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", HW_IF_EFFORT, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
trans.actuator_to_joint( );
EXPECT_THAT( -60.0, DoubleNear( j_val[0], EPS ) );
EXPECT_THAT( -160.0, DoubleNear( j_val[1], EPS ) );
}
// Velocity interface
{
auto a1_handle = ActuatorHandle( "act1", HW_IF_VELOCITY, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", HW_IF_VELOCITY, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", HW_IF_VELOCITY, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", HW_IF_VELOCITY, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
trans.actuator_to_joint( );
EXPECT_THAT( -0.15, DoubleNear( j_val[0], EPS ) );
EXPECT_THAT( -0.025, DoubleNear( j_val[1], EPS ) );
}
// Position interface
{
auto a1_handle = ActuatorHandle( "act1", HW_IF_POSITION, a_vec[0] );
auto a2_handle = ActuatorHandle( "act2", HW_IF_POSITION, a_vec[1] );
auto joint1_handle = JointHandle( "joint1", HW_IF_POSITION, j_vec[0] );
auto joint2_handle = JointHandle( "joint2", HW_IF_POSITION, j_vec[1] );
trans.configure( {joint1_handle, joint2_handle}, {a1_handle, a2_handle} );
trans.actuator_to_joint( );
EXPECT_THAT( -2.15, DoubleNear( j_val[0], EPS ) );
EXPECT_THAT( 3.975, DoubleNear( j_val[1], EPS ) );
}
}
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/// \author Adolfo Rodriguez Tsouroukdissian
#ifndef RANDOM_GENERATOR_UTILS_HPP_
#define RANDOM_GENERATOR_UTILS_HPP_
#include <cstdlib>
#include <ctime>
#include <vector>
using std::vector;
/// \brief Generator of pseudo-random double in the range [min_val, max_val].
// NOTE: Based on example code available at:
// http://stackoverflow.com/questions/2860673/initializing-a-c-vector-to-random-values-fast
struct RandomDoubleGenerator
{
public:
RandomDoubleGenerator( double min_val, double max_val ) : min_val_( min_val ), max_val_( max_val )
{
srand( time( nullptr ) );
}
double operator( )( )
{
const double range = max_val_ - min_val_;
return rand( ) / static_cast<double>( RAND_MAX ) * range + min_val_;
}
private:
double min_val_;
double max_val_;
};
/// \brief Generator of a vector of pseudo-random doubles.
48 vector<double> randomVector( const vector<double>::size_type size, RandomDoubleGenerator & generator )
{
vector<double> out;
out.reserve( size );
for ( vector<double>::size_type i = 0; i < size; ++i )
{
out.push_back( generator( ) );
}
return out;
}
#endif // RANDOM_GENERATOR_UTILS_HPP_
1 // Copyright 2022 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gmock/gmock.h>
#include <exception>
#include <memory>
#include <string>
#include <vector>
#include "hardware_interface/component_parser.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "pluginlib/class_loader.hpp"
#include "transmission_interface/simple_transmission.hpp"
#include "transmission_interface/simple_transmission_loader.hpp"
#include "transmission_interface/transmission_loader.hpp"
using testing::SizeIs;
31 class TransmissionPluginLoader
{
public:
34 std::shared_ptr<transmission_interface::TransmissionLoader> create( const std::string & type )
{
try
{
return class_loader_.createUniqueInstance( type );
}
catch ( std::exception & ex )
{
std::cerr << ex.what( ) << std::endl;
return std::shared_ptr<transmission_interface::TransmissionLoader>( );
}
}
private:
// must keep it alive because instance destroyers need it
49 pluginlib::ClassLoader<transmission_interface::TransmissionLoader> class_loader_ = {
"transmission_interface", "transmission_interface::TransmissionLoader"};
};
53 TEST( SimpleTransmissionLoaderTest, FullSpec )
{
// Parse transmission info
std::string urdf_to_test =
R"(
<?xml version="1.0" encoding="utf-8"?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from minimal_robot.urdf.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="MinimalRobot">
<!-- Used for fixing robot -->
<link name="world"/>
<joint name="base_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="world"/>
<child link="base_link"/>
</joint>
<link name="base_link">
<inertial>
<mass value="0.01"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.2" radius="0.1"/>
</geometry>
<material name="DarkGrey">
84 <color rgba="0.4 0.4 0.4 1.0"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="1" radius="0.1"/>
</geometry>
</collision>
</link>
<joint name="joint1" type="revolute">
<origin rpy="-1.57079632679 0 0" xyz="0 0 0.2"/>
<parent link="base_link"/>
<child link="link1"/>
<limit effort="0.1" lower="-3.14159265359" upper="3.14159265359" velocity="0.2"/>
</joint>
<link name="link1">
<inertial>
<mass value="0.01"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="1" radius="0.1"/>
</geometry>
<material name="DarkGrey">
<color rgba="0.4 0.4 0.4 1.0"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="1" radius="0.1"/>
</geometry>
</collision>
</link>
<joint name="joint2" type="revolute">
<origin rpy="1.57079632679 0 0" xyz="0 0 0.9"/>
<parent link="link1"/>
<child link="link2"/>
<limit effort="0.1" lower="-3.14159265359" upper="3.14159265359" velocity="0.2"/>
</joint>
<link name="link2">
<inertial>
<mass value="0.01"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="1" radius="0.1"/>
</geometry>
<material name="DarkGrey">
<color rgba="0.4 0.4 0.4 1.0"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="1" radius="0.1"/>
</geometry>
</collision>
</link>
<joint name="tool_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 1"/>
<parent link="link2"/>
<child link="tool_link"/>
</joint>
<link name="tool_link">
</link>
<ros2_control name="RRBotModularJoint1" type="actuator">
<hardware>
<plugin>ros2_control_demo_hardware/VelocityActuatorHardware</plugin>
<param name="example_param_write_for_sec">1.23</param>
<param name="example_param_read_for_sec">3</param>
</hardware>
<joint name="joint1">
<command_interface name="velocity">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
<transmission name="transmission1">
<plugin>transmission_interface/SimpleTransmission</plugin>
<joint name="joint1" role="joint1">
<mechanical_reduction>325.949</mechanical_reduction>
</joint>
</transmission>
</ros2_control>
<ros2_control name="RRBotModularJoint2" type="actuator">
<hardware>
<plugin>ros2_control_demo_hardware/VelocityActuatorHardware</plugin>
<param name="example_param_write_for_sec">1.23</param>
<param name="example_param_read_for_sec">3</param>
</hardware>
<joint name="joint2">
<command_interface name="velocity">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
</ros2_control>
<ros2_control name="RRBotModularPositionSensorJoint1" type="sensor">
<hardware>
<plugin>ros2_control_demo_hardware/PositionSensorHardware</plugin>
<param name="example_param_read_for_sec">2</param>
</hardware>
<joint name="joint1">
<state_interface name="position"/>
</joint>
</ros2_control>
<ros2_control name="RRBotModularPositionSensorJoint2" type="sensor">
<hardware>
<plugin>ros2_control_demo_hardware/PositionSensorHardware</plugin>
<param name="example_param_read_for_sec">2</param>
</hardware>
<joint name="joint2">
<state_interface name="position"/>
</joint>
</ros2_control>
</robot>
)";
std::vector<hardware_interface::HardwareInfo> infos =
hardware_interface::parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( infos[0].transmissions, SizeIs( 1 ) );
// Transmission loader
TransmissionPluginLoader loader;
std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
loader.create( infos[0].transmissions[0].type );
ASSERT_TRUE( nullptr != transmission_loader );
std::shared_ptr<transmission_interface::Transmission> transmission;
const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
transmission = transmission_loader->load( info );
ASSERT_TRUE( nullptr != transmission );
ASSERT_STREQ( infos[0].transmissions[0].joints[0].role.c_str( ), "joint1" );
// Validate transmission
transmission_interface::SimpleTransmission * simple_transmission =
dynamic_cast<transmission_interface::SimpleTransmission *>( transmission.get( ) );
ASSERT_TRUE( nullptr != simple_transmission );
EXPECT_EQ( 325.949, simple_transmission->get_actuator_reduction( ) );
EXPECT_EQ( 0.0, simple_transmission->get_joint_offset( ) );
}
TEST( SimpleTransmissionLoaderTest, only_mech_red_specified )
{
std::string urdf_to_test = R"(
<?xml version="1.0"?>
<robot name="robot" xmlns="http://www.ros.org">
<ros2_control name="MinimalSpec" type="actuator">
<joint name="joint1">
<command_interface name="velocity">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
<transmission name="transmission1">
<plugin>transmission_interface/SimpleTransmission</plugin>
<joint name="joint1" role="joint1">
<mechanical_reduction>50</mechanical_reduction>
</joint>
</transmission>
</ros2_control>
</robot>
)";
// Parse transmission info
std::vector<hardware_interface::HardwareInfo> infos =
hardware_interface::parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( infos[0].transmissions, SizeIs( 1 ) );
// Transmission loader
TransmissionPluginLoader loader;
std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
loader.create( infos[0].transmissions[0].type );
ASSERT_TRUE( nullptr != transmission_loader );
std::shared_ptr<transmission_interface::Transmission> transmission;
const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
transmission = transmission_loader->load( info );
ASSERT_TRUE( nullptr != transmission );
// Validate transmission
transmission_interface::SimpleTransmission * simple_transmission =
dynamic_cast<transmission_interface::SimpleTransmission *>( transmission.get( ) );
ASSERT_TRUE( nullptr != simple_transmission );
EXPECT_EQ( 50.0, simple_transmission->get_actuator_reduction( ) );
EXPECT_EQ( 0.0, simple_transmission->get_joint_offset( ) );
}
TEST( SimpleTransmissionLoaderTest, offset_and_mech_red_not_specified )
{
std::string urdf_to_test = R"(
<?xml version="1.0"?>
<robot name="robot" xmlns="http://www.ros.org">
<ros2_control name="InvalidSpec" type="actuator">
<joint name="joint1">
<command_interface name="velocity">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
<transmission name="transmission1">
<plugin>transmission_interface/SimpleTransmission</plugin>
<joint name="joint1" role="joint1">
<!-- Unspecified element -->
</joint>
</transmission>
</ros2_control>
</robot>
)";
// Parse transmission info
std::vector<hardware_interface::HardwareInfo> infos =
hardware_interface::parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( infos[0].transmissions, SizeIs( 1 ) );
// Transmission loader
TransmissionPluginLoader loader;
std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader;
transmission_loader = loader.create( infos[0].transmissions[0].type );
ASSERT_TRUE( nullptr != transmission_loader );
std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
const auto trans = infos[0].transmissions[0];
transmission = transmission_loader->load( trans );
ASSERT_TRUE( nullptr != transmission );
transmission_interface::SimpleTransmission * simple_transmission =
dynamic_cast<transmission_interface::SimpleTransmission *>( transmission.get( ) );
ASSERT_TRUE( nullptr != simple_transmission );
EXPECT_EQ( 1.0, simple_transmission->get_actuator_reduction( ) );
EXPECT_EQ( 0.0, simple_transmission->get_joint_offset( ) );
}
TEST( SimpleTransmissionLoaderTest, mechanical_reduction_not_a_number )
{
std::string urdf_to_test = R"(
<?xml version="1.0"?>
<robot name="robot" xmlns="http://www.ros.org">
<ros2_control name="InvalidSpec" type="actuator">
<joint name="joint2">
<command_interface name="velocity">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
<transmission name="transmission2">
<plugin>transmission_interface/SimpleTransmission</plugin>
<joint name="joint2" role="joint1">
<mechanical_reduction>fifty</mechanical_reduction> <!-- Not a number -->
</joint>
</transmission>
</ros2_control>
</robot>
)";
// Parse transmission info
std::vector<hardware_interface::HardwareInfo> infos =
hardware_interface::parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( infos[0].transmissions, SizeIs( 1 ) );
// Transmission loader
TransmissionPluginLoader loader;
std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader;
transmission_loader = loader.create( infos[0].transmissions[0].type );
ASSERT_TRUE( nullptr != transmission_loader );
std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
const auto trans = infos[0].transmissions[0];
transmission = transmission_loader->load( trans );
ASSERT_TRUE( nullptr != transmission );
transmission_interface::SimpleTransmission * simple_transmission =
dynamic_cast<transmission_interface::SimpleTransmission *>( transmission.get( ) );
ASSERT_TRUE( nullptr != simple_transmission );
// default kicks in for ill-defined values
EXPECT_EQ( 1.0, simple_transmission->get_actuator_reduction( ) );
}
TEST( SimpleTransmissionLoaderTest, offset_ill_defined )
{
std::string urdf_to_test = R"(
<?xml version="1.0"?>
<robot name="robot" xmlns="http://www.ros.org">
<ros2_control name="InvalidSpec" type="actuator">
<joint name="joint3">
<command_interface name="velocity">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
<transmission name="transmission3">
<plugin>transmission_interface/SimpleTransmission</plugin>
<joint name="joint3" role="joint1">
<offset>three</offset> <!-- Not a number -->
<mechanical_reduction>50</mechanical_reduction>
</joint>
</transmission>
</ros2_control>
</robot>
)";
// Parse transmission info
std::vector<hardware_interface::HardwareInfo> infos =
hardware_interface::parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( infos[0].transmissions, SizeIs( 1 ) );
// Transmission loader
TransmissionPluginLoader loader;
std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader;
transmission_loader = loader.create( infos[0].transmissions[0].type );
ASSERT_TRUE( nullptr != transmission_loader );
std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
const auto trans = infos[0].transmissions[0];
transmission = transmission_loader->load( trans );
ASSERT_TRUE( nullptr != transmission );
transmission_interface::SimpleTransmission * simple_transmission =
dynamic_cast<transmission_interface::SimpleTransmission *>( transmission.get( ) );
ASSERT_TRUE( nullptr != simple_transmission );
// default kicks in for ill-defined values
EXPECT_EQ( 0.0, simple_transmission->get_joint_offset( ) );
EXPECT_EQ( 50.0, simple_transmission->get_actuator_reduction( ) );
}
TEST( SimpleTransmissionLoaderTest, mech_red_invalid_value )
{
std::string urdf_to_test = R"(
<?xml version="1.0"?>
<robot name="robot" xmlns="http://www.ros.org">
<ros2_control name="InvalidSpec" type="actuator">
<joint name="joint4">
<command_interface name="velocity">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
<transmission name="transmission4">
<plugin>transmission_interface/SimpleTransmission</plugin>
<joint name="joint4" role="joint1">
<mechanical_reduction>0</mechanical_reduction> <!-- Invalid value -->
</joint>
</transmission>
</ros2_control>
</robot>
)";
// Parse transmission info
std::vector<hardware_interface::HardwareInfo> infos =
hardware_interface::parse_control_resources_from_urdf( urdf_to_test );
ASSERT_THAT( infos[0].transmissions, SizeIs( 1 ) );
// Transmission loader
TransmissionPluginLoader loader;
std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader;
transmission_loader = loader.create( infos[0].transmissions[0].type );
ASSERT_TRUE( nullptr != transmission_loader );
std::shared_ptr<transmission_interface::Transmission> transmission = nullptr;
const auto trans = infos[0].transmissions[0];
transmission = transmission_loader->load( trans );
ASSERT_TRUE( nullptr == transmission );
}
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gmock/gmock.h>
#include <string>
#include <vector>
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "transmission_interface/simple_transmission.hpp"
using hardware_interface::HW_IF_EFFORT;
using hardware_interface::HW_IF_POSITION;
using hardware_interface::HW_IF_VELOCITY;
using std::vector;
using transmission_interface::ActuatorHandle;
using transmission_interface::Exception;
using transmission_interface::JointHandle;
using transmission_interface::SimpleTransmission;
// Floating-point value comparison threshold
const double EPS = 1e-6;
33 TEST( PreconditionsTest, ExceptionThrownWithInvalidParameters )
{
// Invalid instance creation: Transmission cannot have zero reduction
EXPECT_THROW( SimpleTransmission( 0.0 ), Exception );
EXPECT_THROW( SimpleTransmission( 0.0, 1.0 ), Exception );
EXPECT_THROW( SimpleTransmission( 0.0, -1.0 ), Exception );
}
41 TEST( PreconditionsTest, NoExceptionsWithValidParameters )
{
EXPECT_NO_THROW( SimpleTransmission( 1.0 ) );
EXPECT_NO_THROW( SimpleTransmission( 1.0, 1.0 ) );
EXPECT_NO_THROW( SimpleTransmission( -1.0, 1.0 ) );
EXPECT_NO_THROW( SimpleTransmission( 1.0, -1.0 ) );
EXPECT_NO_THROW( SimpleTransmission( -1.0, -1.0 ) );
}
50 TEST( PreconditionsTest, AccessorValidation )
{
SimpleTransmission trans( 2.0, -1.0 );
EXPECT_EQ( 1u, trans.num_actuators( ) );
EXPECT_EQ( 1u, trans.num_joints( ) );
EXPECT_EQ( 2.0, trans.get_actuator_reduction( ) );
EXPECT_EQ( -1.0, trans.get_joint_offset( ) );
}
60 TEST( PreconditionsTest, ConfigureFailsWithInvalidHandles )
{
SimpleTransmission trans( 2.0, -1.0 );
double dummy;
auto actuator_handle = ActuatorHandle( "act1", HW_IF_POSITION, &dummy );
auto actuator2_handle = ActuatorHandle( "act2", HW_IF_POSITION, &dummy );
auto joint_handle = JointHandle( "joint1", HW_IF_POSITION, &dummy );
auto joint2_handle = JointHandle( "joint2", HW_IF_POSITION, &dummy );
EXPECT_THROW( trans.configure( {}, {} ), transmission_interface::Exception );
EXPECT_THROW( trans.configure( {joint_handle}, {} ), transmission_interface::Exception );
EXPECT_THROW( trans.configure( {}, {actuator_handle} ), transmission_interface::Exception );
EXPECT_THROW(
trans.configure( {joint_handle}, {actuator_handle, actuator2_handle} ),
transmission_interface::Exception );
EXPECT_THROW(
trans.configure( {joint_handle, joint2_handle}, {actuator_handle} ),
transmission_interface::Exception );
auto invalid_actuator_handle = ActuatorHandle( "act1", HW_IF_VELOCITY, nullptr );
auto invalid_joint_handle = JointHandle( "joint1", HW_IF_VELOCITY, nullptr );
EXPECT_THROW(
trans.configure( {invalid_joint_handle}, {invalid_actuator_handle} ),
transmission_interface::Exception );
EXPECT_THROW(
trans.configure( {}, {actuator_handle, invalid_actuator_handle} ),
transmission_interface::Exception );
EXPECT_THROW(
trans.configure( {invalid_joint_handle}, {actuator_handle} ), transmission_interface::Exception );
}
93 class TransmissionSetup
{
protected:
// Input/output transmission data
double a_val = 0.0;
double j_val = 0.0;
100 void reset_values( )
{
a_val = 0.0;
j_val = 0.0;
}
};
/// Exercises the actuator->joint->actuator roundtrip, which should yield the identity map.
108 class BlackBoxTest : public TransmissionSetup, public ::testing::TestWithParam<SimpleTransmission>
{
protected:
/**
* \param[in] trans Transmission instance.
* \param[in] interface_name The joint/actuator interface used
* \param[in] ref_val Reference value that will be transformed with the respective forward
* and inverse transmission transformations.
*/
117 void testIdentityMap(
118 SimpleTransmission & trans, const std::string & interface_name, const double ref_val )
{
// Effort interface
{
auto actuator_handle = ActuatorHandle( "act1", interface_name, &a_val );
auto joint_handle = JointHandle( "joint1", interface_name, &j_val );
trans.configure( {joint_handle}, {actuator_handle} );
a_val = ref_val;
trans.actuator_to_joint( );
trans.joint_to_actuator( );
EXPECT_NEAR( ref_val, a_val, EPS );
}
}
};
135 TEST_P( BlackBoxTest, IdentityMap )
{
// Transmission instance
SimpleTransmission trans = GetParam( );
// Test transmission for positive, zero, and negative inputs
testIdentityMap( trans, HW_IF_POSITION, 1.0 );
reset_values( );
testIdentityMap( trans, HW_IF_POSITION, 0.0 );
reset_values( );
testIdentityMap( trans, HW_IF_POSITION, -1.0 );
reset_values( );
testIdentityMap( trans, HW_IF_VELOCITY, 1.0 );
reset_values( );
testIdentityMap( trans, HW_IF_VELOCITY, 0.0 );
reset_values( );
testIdentityMap( trans, HW_IF_VELOCITY, -1.0 );
reset_values( );
testIdentityMap( trans, HW_IF_EFFORT, 1.0 );
reset_values( );
testIdentityMap( trans, HW_IF_EFFORT, 0.0 );
reset_values( );
testIdentityMap( trans, HW_IF_EFFORT, -1.0 );
}
162 INSTANTIATE_TEST_SUITE_P(
IdentityMap, BlackBoxTest,
::testing::Values(
SimpleTransmission( 10.0 ), SimpleTransmission( -10.0 ), SimpleTransmission( 10.0, 1.0 ),
SimpleTransmission( 10.0, -1.0 ), SimpleTransmission( -10.0, 1.0 ),
SimpleTransmission( -10.0, -1.0 ) ) );
169 class WhiteBoxTest : public TransmissionSetup, public ::testing::Test
{
};
173 TEST_F( WhiteBoxTest, MoveJoint )
{
// NOTE: We only test the actuator->joint map
// as the joint->actuator map is indirectly validated in the test that
// checks that actuator->joint->actuator == identity.
SimpleTransmission trans( 10.0, 1.0 );
a_val = 1.0;
// Effort interface
{
auto actuator_handle = ActuatorHandle( "act1", HW_IF_EFFORT, &a_val );
auto joint_handle = JointHandle( "joint1", HW_IF_EFFORT, &j_val );
trans.configure( {joint_handle}, {actuator_handle} );
trans.actuator_to_joint( );
EXPECT_NEAR( 10.0, j_val, EPS );
}
// Velocity interface
{
auto actuator_handle = ActuatorHandle( "act1", HW_IF_VELOCITY, &a_val );
auto joint_handle = JointHandle( "joint1", HW_IF_VELOCITY, &j_val );
trans.configure( {joint_handle}, {actuator_handle} );
trans.actuator_to_joint( );
EXPECT_NEAR( 0.1, j_val, EPS );
}
// Position interface
{
auto actuator_handle = ActuatorHandle( "act1", HW_IF_POSITION, &a_val );
auto joint_handle = JointHandle( "joint1", HW_IF_POSITION, &j_val );
trans.configure( {joint_handle}, {actuator_handle} );
trans.actuator_to_joint( );
EXPECT_NEAR( 1.1, j_val, EPS );
}
// Mismatched interface is ignored
{
double unique_value = 13.37;
auto actuator_handle = ActuatorHandle( "act1", HW_IF_POSITION, &a_val );
auto actuator_handle2 = ActuatorHandle( "act1", HW_IF_VELOCITY, &unique_value );
auto joint_handle = JointHandle( "joint1", HW_IF_POSITION, &j_val );
auto joint_handle2 = JointHandle( "joint1", HW_IF_VELOCITY, &unique_value );
trans.configure( {joint_handle, joint_handle2}, {actuator_handle} );
trans.actuator_to_joint( );
EXPECT_NEAR( unique_value, 13.37, EPS );
trans.configure( {joint_handle}, {actuator_handle, actuator_handle2} );
trans.actuator_to_joint( );
EXPECT_NEAR( unique_value, 13.37, EPS );
}
}
// Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gmock/gmock.h>
#include <string>
#include <vector>
#include "transmission_interface/transmission_parser.hpp"
using namespace ::testing; // NOLINT
23 class TestTransmissionParser : public Test
{
public:
void SetUp( ) override
{
valid_urdf_xml_ =
R"(
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from rrbot.urdf.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="rrbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Build your comprehensive robot -->
<link name="world"/>
<gazebo reference="world">
<static>true</static>
</gazebo>
<joint name="rrbot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="world"/>
<child link="rrbot_link1"/>
</joint>
<link name="rrbot_link1">
<collision>
<origin rpy="0 0 0" xyz="0 0 1.0"/>
<geometry>
<box size="0.1 0.1 2"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 1.0"/>
<geometry>
<box size="0.1 0.1 2"/>
</geometry>
<material name="RRBOT/orange"/>
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0 1.0"/>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="rrbot_joint1" type="continuous">
<parent link="rrbot_link1"/>
<child link="rrbot_link2"/>
<origin rpy="0 0 0" xyz="0 0.1 1.95"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.7"/>
</joint>
<link name="rrbot_link2">
<collision>
<origin rpy="0 0 0" xyz="0 0 0.45"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.45"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
<material name="RRBOT/black"/>
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.45"/>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="rrbot_joint2" type="continuous">
<parent link="rrbot_link2"/>
<child link="rrbot_link3"/>
<origin rpy="0 0 0" xyz="0 0.1 0.9"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.7"/>
</joint>
<link name="rrbot_link3">
<collision>
<origin rpy="0 0 0" xyz="0 0 0.45"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.45"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
<material name="RRBOT/orange"/>
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.45"/>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="rrbot_hokuyo_joint" type="fixed">
<axis xyz="0 1 0"/>
<origin rpy="0 0 0" xyz="0 0 0.975"/>
<parent link="rrbot_link3"/>
<child link="rrbot_hokuyo_link"/>
</joint>
<link name="rrbot_hokuyo_link">
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</visual>
<inertial>
<mass value="1e-5"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
</link>
<joint name="rrbot_camera_joint" type="fixed">
<axis xyz="0 1 0"/>
<origin rpy="0 0 0" xyz="0.05 0 0.9"/>
<parent link="rrbot_link3"/>
<child link="rrbot_camera_link"/>
</joint>
<link name="rrbot_camera_link">
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
<material name="RRBOT/red"/>
</visual>
<inertial>
<mass value="1e-5"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
</link>
<transmission name="rrbot_tran1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="rrbot_joint1">
<hardwareInterface>PositionJointInterface</hardwareInterface>
</joint>
<actuator name="rrbot_motor1">
<mechanicalReduction>1</mechanicalReduction>
<hardwareInterface>PositionJointInterface</hardwareInterface>
</actuator>
</transmission>
<transmission name="rrbot_tran2">
<type>transmission_interface/SimpleTransmission</type>
<joint name="rrbot_joint2">
<hardwareInterface>VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="rrbot_motor2">
<mechanicalReduction>60</mechanicalReduction>
<hardwareInterface>VelocityJointInterface</hardwareInterface>
</actuator>
</transmission>
<gazebo reference="rrbot_link1">
<material>Gazebo/Orange</material>
</gazebo>
<gazebo reference="rrbot_link2">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="rrbot_link3">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Orange</material>
</gazebo>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
</plugin>
</gazebo>
</robot>
)";
wrong_urdf_xml_ =
R"(
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from rrbot.urdf.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="rrbot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Build your comprehensive robot -->
<link name="world"/>
<gazebo reference="world">
<static>true</static>
</gazebo>
<joint name="rrbot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="world"/>
<child link="rrbot_link1"/>
</joint>
<link name="rrbot_link1">
<collision>
<origin rpy="0 0 0" xyz="0 0 1.0"/>
<geometry>
<box size="0.1 0.1 2"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 1.0"/>
<geometry>
<box size="0.1 0.1 2"/>
</geometry>
<material name="RRBOT/orange"/>
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0 1.0"/>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="rrbot_joint1" type="continuous">
<parent link="rrbot_link1"/>
<child link="rrbot_link2"/>
<origin rpy="0 0 0" xyz="0 0.1 1.95"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.7"/>
</joint>
<link name="rrbot_link2">
<collision>
<origin rpy="0 0 0" xyz="0 0 0.45"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.45"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
<material name="RRBOT/black"/>
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.45"/>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="rrbot_joint2" type="continuous">
<parent link="rrbot_link2"/>
<child link="rrbot_link3"/>
<origin rpy="0 0 0" xyz="0 0.1 0.9"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.7"/>
</joint>
<link name="rrbot_link3">
<collision>
<origin rpy="0 0 0" xyz="0 0 0.45"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.45"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
<material name="RRBOT/orange"/>
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.45"/>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="rrbot_hokuyo_joint" type="fixed">
<axis xyz="0 1 0"/>
<origin rpy="0 0 0" xyz="0 0 0.975"/>
<parent link="rrbot_link3"/>
<child link="rrbot_hokuyo_link"/>
</joint>
<link name="rrbot_hokuyo_link">
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</visual>
<inertial>
<mass value="1e-5"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
</link>
<joint name="rrbot_camera_joint" type="fixed">
<axis xyz="0 1 0"/>
<origin rpy="0 0 0" xyz="0.05 0 0.9"/>
<parent link="rrbot_link3"/>
<child link="rrbot_camera_link"/>
</joint>
<link name="rrbot_camera_link">
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
<material name="RRBOT/red"/>
</visual>
<inertial>
<mass value="1e-5"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
</link>
<transmission name="rrbot_tran1">
<type>transmission_interface/SimpleTransmission</type>
<actuator name="rrbot_motor1">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<gazebo reference="rrbot_link1">
<material>Gazebo/Orange</material>
</gazebo>
<gazebo reference="rrbot_link2">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Black</material>
</gazebo>
<gazebo reference="rrbot_link3">
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Orange</material>
</gazebo>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
</plugin>
</gazebo>
</robot>
)";
}
std::string valid_urdf_xml_;
std::string wrong_urdf_xml_;
};
using transmission_interface::parse_transmissions_from_urdf;
using transmission_interface::TransmissionInfo;
TEST_F( TestTransmissionParser, successfully_parse_valid_urdf )
{
const auto transmissions = parse_transmissions_from_urdf( valid_urdf_xml_ );
ASSERT_THAT( transmissions, SizeIs( 2 ) );
// first transmission
EXPECT_EQ( "rrbot_tran1", transmissions[0].name );
EXPECT_EQ( "transmission_interface/SimpleTransmission", transmissions[0].type );
ASSERT_THAT( transmissions[0].joints, SizeIs( 1 ) );
ASSERT_THAT( transmissions[0].joints[0].interfaces, SizeIs( 1 ) );
EXPECT_EQ( "rrbot_joint1", transmissions[0].joints[0].name );
EXPECT_EQ( "PositionJointInterface", transmissions[0].joints[0].interfaces[0] );
ASSERT_THAT( transmissions[0].actuators, SizeIs( 1 ) );
ASSERT_THAT( transmissions[0].actuators[0].interfaces, SizeIs( 1 ) );
EXPECT_EQ( "rrbot_motor1", transmissions[0].actuators[0].name );
EXPECT_EQ( "PositionJointInterface", transmissions[0].actuators[0].interfaces[0] );
EXPECT_EQ( 1, transmissions[0].actuators[0].mechanical_reduction );
// second transmission
EXPECT_EQ( "rrbot_tran2", transmissions[1].name );
EXPECT_EQ( "transmission_interface/SimpleTransmission", transmissions[1].type );
ASSERT_THAT( transmissions[1].joints, SizeIs( 1 ) );
ASSERT_THAT( transmissions[1].joints[0].interfaces, SizeIs( 1 ) );
EXPECT_EQ( "rrbot_joint2", transmissions[1].joints[0].name );
EXPECT_EQ( "VelocityJointInterface", transmissions[1].joints[0].interfaces[0] );
ASSERT_THAT( transmissions[1].actuators, SizeIs( 1 ) );
ASSERT_THAT( transmissions[1].actuators[0].interfaces, SizeIs( 1 ) );
EXPECT_EQ( "rrbot_motor2", transmissions[1].actuators[0].name );
EXPECT_EQ( "VelocityJointInterface", transmissions[1].actuators[0].interfaces[0] );
EXPECT_EQ( 60, transmissions[1].actuators[0].mechanical_reduction );
}
TEST_F( TestTransmissionParser, empty_string_throws_error )
{
ASSERT_THROW( transmission_interface::parse_transmissions_from_urdf( "" ), std::runtime_error );
}
TEST_F( TestTransmissionParser, empty_urdf_returns_empty )
{
const auto transmissions = transmission_interface::parse_transmissions_from_urdf(
"<?xml version=\"1.0\"?><robot name=\"robot\" xmlns=\"http://www.ros.org\"></robot>" );
ASSERT_THAT( transmissions, IsEmpty( ) );
}
TEST_F( TestTransmissionParser, wrong_urdf_throws_error )
{
EXPECT_THROW(
transmission_interface::parse_transmissions_from_urdf( wrong_urdf_xml_ ), std::runtime_error );
}
// Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*
* Author: Bence Magyar, Enrique Fernández, Manuel Meraz
*/
#ifndef DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_
#define DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_
#include <chrono>
#include <cmath>
#include <memory>
#include <queue>
#include <string>
#include <vector>
#include "controller_interface/controller_interface.hpp"
#include "diff_drive_controller/odometry.hpp"
#include "diff_drive_controller/speed_limiter.hpp"
#include "diff_drive_controller/visibility_control.h"
#include "geometry_msgs/msg/twist.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
#include "hardware_interface/handle.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "odometry.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "realtime_tools/realtime_box.h"
#include "realtime_tools/realtime_buffer.h"
#include "realtime_tools/realtime_publisher.h"
#include "tf2_msgs/msg/tf_message.hpp"
namespace diff_drive_controller
{
47 class DiffDriveController : public controller_interface::ControllerInterface
{
using Twist = geometry_msgs::msg::TwistStamped;
public:
DIFF_DRIVE_CONTROLLER_PUBLIC
DiffDriveController( );
DIFF_DRIVE_CONTROLLER_PUBLIC
controller_interface::InterfaceConfiguration command_interface_configuration( ) const override;
DIFF_DRIVE_CONTROLLER_PUBLIC
controller_interface::InterfaceConfiguration state_interface_configuration( ) const override;
DIFF_DRIVE_CONTROLLER_PUBLIC
controller_interface::return_type update(
const rclcpp::Time & time, const rclcpp::Duration & period ) override;
DIFF_DRIVE_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_init( ) override;
DIFF_DRIVE_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_configure(
const rclcpp_lifecycle::State & previous_state ) override;
DIFF_DRIVE_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_activate(
const rclcpp_lifecycle::State & previous_state ) override;
DIFF_DRIVE_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_deactivate(
const rclcpp_lifecycle::State & previous_state ) override;
DIFF_DRIVE_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_cleanup(
const rclcpp_lifecycle::State & previous_state ) override;
DIFF_DRIVE_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_error(
const rclcpp_lifecycle::State & previous_state ) override;
DIFF_DRIVE_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_shutdown(
const rclcpp_lifecycle::State & previous_state ) override;
protected:
struct WheelHandle
{
std::reference_wrapper<const hardware_interface::LoanedStateInterface> feedback;
std::reference_wrapper<hardware_interface::LoanedCommandInterface> velocity;
};
99 const char * feedback_type( ) const;
100 controller_interface::CallbackReturn configure_side(
const std::string & side, const std::vector<std::string> & wheel_names,
std::vector<WheelHandle> & registered_handles );
std::vector<std::string> left_wheel_names_;
std::vector<std::string> right_wheel_names_;
std::vector<WheelHandle> registered_left_wheel_handles_;
std::vector<WheelHandle> registered_right_wheel_handles_;
struct WheelParams
{
size_t wheels_per_side = 0;
double separation = 0.0; // w.r.t. the midpoint of the wheel width
double radius = 0.0; // Assumed to be the same for both wheels
double separation_multiplier = 1.0;
double left_radius_multiplier = 1.0;
double right_radius_multiplier = 1.0;
} wheel_params_;
struct OdometryParams
{
bool open_loop = false;
bool position_feedback = true;
bool enable_odom_tf = true;
std::string base_frame_id = "base_link";
std::string odom_frame_id = "odom";
std::array<double, 6> pose_covariance_diagonal;
std::array<double, 6> twist_covariance_diagonal;
} odom_params_;
Odometry odometry_;
133 std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> odometry_publisher_ = nullptr;
134 std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>
realtime_odometry_publisher_ = nullptr;
137 std::shared_ptr<rclcpp::Publisher<tf2_msgs::msg::TFMessage>> odometry_transform_publisher_ =
nullptr;
139 std::shared_ptr<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>>
realtime_odometry_transform_publisher_ = nullptr;
// Timeout to consider cmd_vel commands old
std::chrono::milliseconds cmd_vel_timeout_{500};
bool subscriber_is_active_ = false;
rclcpp::Subscription<Twist>::SharedPtr velocity_command_subscriber_ = nullptr;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr
velocity_command_unstamped_subscriber_ = nullptr;
realtime_tools::RealtimeBox<std::shared_ptr<Twist>> received_velocity_msg_ptr_{nullptr};
std::queue<Twist> previous_commands_; // last two commands
// speed limiters
SpeedLimiter limiter_linear_;
SpeedLimiter limiter_angular_;
bool publish_limited_velocity_ = false;
std::shared_ptr<rclcpp::Publisher<Twist>> limited_velocity_publisher_ = nullptr;
std::shared_ptr<realtime_tools::RealtimePublisher<Twist>> realtime_limited_velocity_publisher_ =
nullptr;
rclcpp::Time previous_update_timestamp_{0};
// publish rate limiter
double publish_rate_ = 50.0;
rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds( 0 );
rclcpp::Time previous_publish_timestamp_{0};
bool is_halted = false;
bool use_stamped_vel_ = true;
bool reset( );
void halt( );
};
} // namespace diff_drive_controller
#endif // DIFF_DRIVE_CONTROLLER__DIFF_DRIVE_CONTROLLER_HPP_
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*
* Author: Luca Marchionni
* Author: Bence Magyar
* Author: Enrique Fernández
* Author: Paul Mathieu
*/
#ifndef DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_
#define DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_
#include <cmath>
#include "rclcpp/time.hpp"
#include "rcppmath/rolling_mean_accumulator.hpp"
namespace diff_drive_controller
{
32 class Odometry
{
public:
35 explicit Odometry( size_t velocity_rolling_window_size = 10 );
37 void init( const rclcpp::Time & time );
38 bool update( double left_pos, double right_pos, const rclcpp::Time & time );
39 bool updateFromVelocity( double left_vel, double right_vel, const rclcpp::Time & time );
40 void updateOpenLoop( double linear, double angular, const rclcpp::Time & time );
41 void resetOdometry( );
43 double getX( ) const { return x_; }
44 double getY( ) const { return y_; }
45 double getHeading( ) const { return heading_; }
46 double getLinear( ) const { return linear_; }
47 double getAngular( ) const { return angular_; }
49 void setWheelParams( double wheel_separation, double left_wheel_radius, double right_wheel_radius );
50 void setVelocityRollingWindowSize( size_t velocity_rolling_window_size );
private:
using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator<double>;
void integrateRungeKutta2( double linear, double angular );
void integrateExact( double linear, double angular );
void resetAccumulators( );
// Current timestamp:
rclcpp::Time timestamp_;
// Current pose:
double x_; // [m]
double y_; // [m]
double heading_; // [rad]
// Current velocity:
double linear_; // [m/s]
double angular_; // [rad/s]
// Wheel kinematic parameters [m]:
double wheel_separation_;
double left_wheel_radius_;
double right_wheel_radius_;
// Previous wheel position/state [rad]:
double left_wheel_old_pos_;
double right_wheel_old_pos_;
// Rolling mean accumulators for the linear and angular velocities:
size_t velocity_rolling_window_size_;
RollingMeanAccumulator linear_accumulator_;
RollingMeanAccumulator angular_accumulator_;
};
} // namespace diff_drive_controller
#endif // DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*
* Author: Enrique Fernández
*/
#ifndef DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_
#define DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_
#include <cmath>
namespace diff_drive_controller
{
26 class SpeedLimiter
{
public:
/**
* \brief Constructor
* \param [in] has_velocity_limits if true, applies velocity limits
* \param [in] has_acceleration_limits if true, applies acceleration limits
* \param [in] has_jerk_limits if true, applies jerk limits
* \param [in] min_velocity Minimum velocity [m/s], usually <= 0
* \param [in] max_velocity Maximum velocity [m/s], usually >= 0
* \param [in] min_acceleration Minimum acceleration [m/s^2], usually <= 0
* \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0
* \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0
* \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0
*/
41 SpeedLimiter(
bool has_velocity_limits = false, bool has_acceleration_limits = false,
bool has_jerk_limits = false, double min_velocity = NAN, double max_velocity = NAN,
double min_acceleration = NAN, double max_acceleration = NAN, double min_jerk = NAN,
double max_jerk = NAN );
/**
* \brief Limit the velocity and acceleration
* \param [in, out] v Velocity [m/s]
* \param [in] v0 Previous velocity to v [m/s]
* \param [in] v1 Previous velocity to v0 [m/s]
* \param [in] dt Time step [s]
* \return Limiting factor ( 1.0 if none )
*/
55 double limit( double & v, double v0, double v1, double dt );
/**
* \brief Limit the velocity
* \param [in, out] v Velocity [m/s]
* \return Limiting factor ( 1.0 if none )
*/
62 double limit_velocity( double & v );
/**
* \brief Limit the acceleration
* \param [in, out] v Velocity [m/s]
* \param [in] v0 Previous velocity [m/s]
* \param [in] dt Time step [s]
* \return Limiting factor ( 1.0 if none )
*/
71 double limit_acceleration( double & v, double v0, double dt );
/**
* \brief Limit the jerk
* \param [in, out] v Velocity [m/s]
* \param [in] v0 Previous velocity to v [m/s]
* \param [in] v1 Previous velocity to v0 [m/s]
* \param [in] dt Time step [s]
* \return Limiting factor ( 1.0 if none )
* \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control
*/
82 double limit_jerk( double & v, double v0, double v1, double dt );
private:
// Enable/Disable velocity/acceleration/jerk limits:
86 bool has_velocity_limits_;
87 bool has_acceleration_limits_;
88 bool has_jerk_limits_;
// Velocity limits:
double min_velocity_;
double max_velocity_;
// Acceleration limits:
double min_acceleration_;
double max_acceleration_;
// Jerk limits:
double min_jerk_;
double max_jerk_;
};
} // namespace diff_drive_controller
#endif // DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_
1 // Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/* This header must be included by all rclcpp headers which declare symbols
* which are defined in the rclcpp library. When not building the rclcpp
* library, i.e. when using the headers in other package's code, the contents
* of this header change the visibility of certain symbols which the rclcpp
* library cannot have, but the consuming code must have inorder to link.
*/
#ifndef DIFF_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_
#define DIFF_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_
// This logic was borrowed ( then namespaced ) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define DIFF_DRIVE_CONTROLLER_EXPORT __attribute__( ( dllexport ) )
#define DIFF_DRIVE_CONTROLLER_IMPORT __attribute__( ( dllimport ) )
#else
#define DIFF_DRIVE_CONTROLLER_EXPORT __declspec( dllexport )
#define DIFF_DRIVE_CONTROLLER_IMPORT __declspec( dllimport )
#endif
#ifdef DIFF_DRIVE_CONTROLLER_BUILDING_DLL
#define DIFF_DRIVE_CONTROLLER_PUBLIC DIFF_DRIVE_CONTROLLER_EXPORT
#else
#define DIFF_DRIVE_CONTROLLER_PUBLIC DIFF_DRIVE_CONTROLLER_IMPORT
#endif
#define DIFF_DRIVE_CONTROLLER_PUBLIC_TYPE DIFF_DRIVE_CONTROLLER_PUBLIC
#define DIFF_DRIVE_CONTROLLER_LOCAL
#else
#define DIFF_DRIVE_CONTROLLER_EXPORT __attribute__( ( visibility( "default" ) ) )
#define DIFF_DRIVE_CONTROLLER_IMPORT
#if __GNUC__ >= 4
#define DIFF_DRIVE_CONTROLLER_PUBLIC __attribute__( ( visibility( "default" ) ) )
#define DIFF_DRIVE_CONTROLLER_LOCAL __attribute__( ( visibility( "hidden" ) ) )
#else
#define DIFF_DRIVE_CONTROLLER_PUBLIC
#define DIFF_DRIVE_CONTROLLER_LOCAL
#endif
#define DIFF_DRIVE_CONTROLLER_PUBLIC_TYPE
#endif
#endif // DIFF_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_
// Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*
* Author: Bence Magyar, Enrique Fernández, Manuel Meraz
*/
#include <memory>
#include <queue>
#include <string>
#include <utility>
#include <vector>
#include "diff_drive_controller/diff_drive_controller.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/logging.hpp"
#include "tf2/LinearMath/Quaternion.h"
namespace
{
33 constexpr auto DEFAULT_COMMAND_TOPIC = "~/cmd_vel";
constexpr auto DEFAULT_COMMAND_UNSTAMPED_TOPIC = "~/cmd_vel_unstamped";
constexpr auto DEFAULT_COMMAND_OUT_TOPIC = "~/cmd_vel_out";
constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom";
constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf";
} // namespace
namespace diff_drive_controller
{
using namespace std::chrono_literals;
using controller_interface::interface_configuration_type;
using controller_interface::InterfaceConfiguration;
using hardware_interface::HW_IF_POSITION;
using hardware_interface::HW_IF_VELOCITY;
using lifecycle_msgs::msg::State;
DiffDriveController::DiffDriveController( ) : controller_interface::ControllerInterface( ) {}
const char * DiffDriveController::feedback_type( ) const
{
return odom_params_.position_feedback ? HW_IF_POSITION : HW_IF_VELOCITY;
}
controller_interface::CallbackReturn DiffDriveController::on_init( )
{
try
{
// with the lifecycle node being initialized, we can declare parameters
auto_declare<std::vector<std::string>>( "left_wheel_names", std::vector<std::string>( ) );
auto_declare<std::vector<std::string>>( "right_wheel_names", std::vector<std::string>( ) );
auto_declare<double>( "wheel_separation", wheel_params_.separation );
auto_declare<int>( "wheels_per_side", wheel_params_.wheels_per_side );
auto_declare<double>( "wheel_radius", wheel_params_.radius );
auto_declare<double>( "wheel_separation_multiplier", wheel_params_.separation_multiplier );
auto_declare<double>( "left_wheel_radius_multiplier", wheel_params_.left_radius_multiplier );
auto_declare<double>( "right_wheel_radius_multiplier", wheel_params_.right_radius_multiplier );
auto_declare<std::string>( "odom_frame_id", odom_params_.odom_frame_id );
auto_declare<std::string>( "base_frame_id", odom_params_.base_frame_id );
auto_declare<std::vector<double>>( "pose_covariance_diagonal", std::vector<double>( ) );
auto_declare<std::vector<double>>( "twist_covariance_diagonal", std::vector<double>( ) );
auto_declare<bool>( "open_loop", odom_params_.open_loop );
auto_declare<bool>( "position_feedback", odom_params_.position_feedback );
auto_declare<bool>( "enable_odom_tf", odom_params_.enable_odom_tf );
auto_declare<double>( "cmd_vel_timeout", cmd_vel_timeout_.count( ) / 1000.0 );
auto_declare<bool>( "publish_limited_velocity", publish_limited_velocity_ );
auto_declare<int>( "velocity_rolling_window_size", 10 );
auto_declare<bool>( "use_stamped_vel", use_stamped_vel_ );
auto_declare<bool>( "linear.x.has_velocity_limits", false );
auto_declare<bool>( "linear.x.has_acceleration_limits", false );
auto_declare<bool>( "linear.x.has_jerk_limits", false );
auto_declare<double>( "linear.x.max_velocity", NAN );
auto_declare<double>( "linear.x.min_velocity", NAN );
auto_declare<double>( "linear.x.max_acceleration", NAN );
auto_declare<double>( "linear.x.min_acceleration", NAN );
auto_declare<double>( "linear.x.max_jerk", NAN );
auto_declare<double>( "linear.x.min_jerk", NAN );
auto_declare<bool>( "angular.z.has_velocity_limits", false );
auto_declare<bool>( "angular.z.has_acceleration_limits", false );
auto_declare<bool>( "angular.z.has_jerk_limits", false );
auto_declare<double>( "angular.z.max_velocity", NAN );
auto_declare<double>( "angular.z.min_velocity", NAN );
auto_declare<double>( "angular.z.max_acceleration", NAN );
auto_declare<double>( "angular.z.min_acceleration", NAN );
auto_declare<double>( "angular.z.max_jerk", NAN );
auto_declare<double>( "angular.z.min_jerk", NAN );
auto_declare<double>( "publish_rate", publish_rate_ );
}
catch ( const std::exception & e )
{
fprintf( stderr, "Exception thrown during init stage with message: %s \n", e.what( ) );
return controller_interface::CallbackReturn::ERROR;
}
return controller_interface::CallbackReturn::SUCCESS;
}
InterfaceConfiguration DiffDriveController::command_interface_configuration( ) const
{
std::vector<std::string> conf_names;
for ( const auto & joint_name : left_wheel_names_ )
{
conf_names.push_back( joint_name + "/" + HW_IF_VELOCITY );
}
for ( const auto & joint_name : right_wheel_names_ )
{
conf_names.push_back( joint_name + "/" + HW_IF_VELOCITY );
}
return {interface_configuration_type::INDIVIDUAL, conf_names};
}
InterfaceConfiguration DiffDriveController::state_interface_configuration( ) const
{
std::vector<std::string> conf_names;
for ( const auto & joint_name : left_wheel_names_ )
{
conf_names.push_back( joint_name + "/" + feedback_type( ) );
}
for ( const auto & joint_name : right_wheel_names_ )
{
conf_names.push_back( joint_name + "/" + feedback_type( ) );
}
return {interface_configuration_type::INDIVIDUAL, conf_names};
}
controller_interface::return_type DiffDriveController::update(
const rclcpp::Time & time, const rclcpp::Duration & period )
{
auto logger = get_node( )->get_logger( );
if ( get_state( ).id( ) == State::PRIMARY_STATE_INACTIVE )
{
if ( !is_halted )
{
halt( );
is_halted = true;
}
return controller_interface::return_type::OK;
}
std::shared_ptr<Twist> last_command_msg;
received_velocity_msg_ptr_.get( last_command_msg );
if ( last_command_msg == nullptr )
{
RCLCPP_WARN( logger, "Velocity message received was a nullptr." );
return controller_interface::return_type::ERROR;
}
const auto age_of_last_command = time - last_command_msg->header.stamp;
// Brake if cmd_vel has timeout, override the stored command
if ( age_of_last_command > cmd_vel_timeout_ )
{
last_command_msg->twist.linear.x = 0.0;
last_command_msg->twist.angular.z = 0.0;
}
// command may be limited further by SpeedLimit,
// without affecting the stored twist command
Twist command = *last_command_msg;
double & linear_command = command.twist.linear.x;
double & angular_command = command.twist.angular.z;
previous_update_timestamp_ = time;
// Apply ( possibly new ) multipliers:
const auto wheels = wheel_params_;
const double wheel_separation = wheels.separation_multiplier * wheels.separation;
const double left_wheel_radius = wheels.left_radius_multiplier * wheels.radius;
const double right_wheel_radius = wheels.right_radius_multiplier * wheels.radius;
if ( odom_params_.open_loop )
{
odometry_.updateOpenLoop( linear_command, angular_command, time );
}
else
{
double left_feedback_mean = 0.0;
double right_feedback_mean = 0.0;
for ( size_t index = 0; index < wheels.wheels_per_side; ++index )
{
const double left_feedback = registered_left_wheel_handles_[index].feedback.get( ).get_value( );
const double right_feedback =
registered_right_wheel_handles_[index].feedback.get( ).get_value( );
if ( std::isnan( left_feedback ) || std::isnan( right_feedback ) )
{
RCLCPP_ERROR(
logger, "Either the left or right wheel %s is invalid for index [%zu]", feedback_type( ),
index );
return controller_interface::return_type::ERROR;
}
left_feedback_mean += left_feedback;
right_feedback_mean += right_feedback;
}
left_feedback_mean /= wheels.wheels_per_side;
right_feedback_mean /= wheels.wheels_per_side;
if ( odom_params_.position_feedback )
{
odometry_.update( left_feedback_mean, right_feedback_mean, time );
}
else
{
odometry_.updateFromVelocity(
left_feedback_mean * period.seconds( ), right_feedback_mean * period.seconds( ), time );
}
}
tf2::Quaternion orientation;
orientation.setRPY( 0.0, 0.0, odometry_.getHeading( ) );
if ( previous_publish_timestamp_ + publish_period_ < time )
{
previous_publish_timestamp_ += publish_period_;
if ( realtime_odometry_publisher_->trylock( ) )
{
auto & odometry_message = realtime_odometry_publisher_->msg_;
odometry_message.header.stamp = time;
odometry_message.pose.pose.position.x = odometry_.getX( );
odometry_message.pose.pose.position.y = odometry_.getY( );
odometry_message.pose.pose.orientation.x = orientation.x( );
odometry_message.pose.pose.orientation.y = orientation.y( );
odometry_message.pose.pose.orientation.z = orientation.z( );
odometry_message.pose.pose.orientation.w = orientation.w( );
odometry_message.twist.twist.linear.x = odometry_.getLinear( );
odometry_message.twist.twist.angular.z = odometry_.getAngular( );
realtime_odometry_publisher_->unlockAndPublish( );
}
if ( odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock( ) )
{
auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front( );
transform.header.stamp = time;
transform.transform.translation.x = odometry_.getX( );
transform.transform.translation.y = odometry_.getY( );
transform.transform.rotation.x = orientation.x( );
transform.transform.rotation.y = orientation.y( );
transform.transform.rotation.z = orientation.z( );
transform.transform.rotation.w = orientation.w( );
realtime_odometry_transform_publisher_->unlockAndPublish( );
}
}
auto & last_command = previous_commands_.back( ).twist;
auto & second_to_last_command = previous_commands_.front( ).twist;
limiter_linear_.limit(
linear_command, last_command.linear.x, second_to_last_command.linear.x, period.seconds( ) );
limiter_angular_.limit(
angular_command, last_command.angular.z, second_to_last_command.angular.z, period.seconds( ) );
previous_commands_.pop( );
previous_commands_.emplace( command );
// Publish limited velocity
if ( publish_limited_velocity_ && realtime_limited_velocity_publisher_->trylock( ) )
{
auto & limited_velocity_command = realtime_limited_velocity_publisher_->msg_;
limited_velocity_command.header.stamp = time;
limited_velocity_command.twist = command.twist;
realtime_limited_velocity_publisher_->unlockAndPublish( );
}
// Compute wheels velocities:
const double velocity_left =
( linear_command - angular_command * wheel_separation / 2.0 ) / left_wheel_radius;
const double velocity_right =
( linear_command + angular_command * wheel_separation / 2.0 ) / right_wheel_radius;
// Set wheels velocities:
for ( size_t index = 0; index < wheels.wheels_per_side; ++index )
{
registered_left_wheel_handles_[index].velocity.get( ).set_value( velocity_left );
registered_right_wheel_handles_[index].velocity.get( ).set_value( velocity_right );
}
return controller_interface::return_type::OK;
}
controller_interface::CallbackReturn DiffDriveController::on_configure(
const rclcpp_lifecycle::State & )
{
auto logger = get_node( )->get_logger( );
// update parameters
left_wheel_names_ = get_node( )->get_parameter( "left_wheel_names" ).as_string_array( );
right_wheel_names_ = get_node( )->get_parameter( "right_wheel_names" ).as_string_array( );
if ( left_wheel_names_.size( ) != right_wheel_names_.size( ) )
{
RCLCPP_ERROR(
logger, "The number of left wheels [%zu] and the number of right wheels [%zu] are different",
left_wheel_names_.size( ), right_wheel_names_.size( ) );
return controller_interface::CallbackReturn::ERROR;
}
if ( left_wheel_names_.empty( ) )
{
RCLCPP_ERROR( logger, "Wheel names parameters are empty!" );
return controller_interface::CallbackReturn::ERROR;
}
wheel_params_.separation = get_node( )->get_parameter( "wheel_separation" ).as_double( );
wheel_params_.wheels_per_side =
static_cast<size_t>( get_node( )->get_parameter( "wheels_per_side" ).as_int( ) );
wheel_params_.radius = get_node( )->get_parameter( "wheel_radius" ).as_double( );
wheel_params_.separation_multiplier =
get_node( )->get_parameter( "wheel_separation_multiplier" ).as_double( );
wheel_params_.left_radius_multiplier =
get_node( )->get_parameter( "left_wheel_radius_multiplier" ).as_double( );
wheel_params_.right_radius_multiplier =
get_node( )->get_parameter( "right_wheel_radius_multiplier" ).as_double( );
const auto wheels = wheel_params_;
const double wheel_separation = wheels.separation_multiplier * wheels.separation;
const double left_wheel_radius = wheels.left_radius_multiplier * wheels.radius;
const double right_wheel_radius = wheels.right_radius_multiplier * wheels.radius;
odometry_.setWheelParams( wheel_separation, left_wheel_radius, right_wheel_radius );
odometry_.setVelocityRollingWindowSize(
get_node( )->get_parameter( "velocity_rolling_window_size" ).as_int( ) );
odom_params_.odom_frame_id = get_node( )->get_parameter( "odom_frame_id" ).as_string( );
odom_params_.base_frame_id = get_node( )->get_parameter( "base_frame_id" ).as_string( );
auto pose_diagonal = get_node( )->get_parameter( "pose_covariance_diagonal" ).as_double_array( );
std::copy(
pose_diagonal.begin( ), pose_diagonal.end( ), odom_params_.pose_covariance_diagonal.begin( ) );
auto twist_diagonal = get_node( )->get_parameter( "twist_covariance_diagonal" ).as_double_array( );
std::copy(
twist_diagonal.begin( ), twist_diagonal.end( ), odom_params_.twist_covariance_diagonal.begin( ) );
odom_params_.open_loop = get_node( )->get_parameter( "open_loop" ).as_bool( );
odom_params_.position_feedback = get_node( )->get_parameter( "position_feedback" ).as_bool( );
odom_params_.enable_odom_tf = get_node( )->get_parameter( "enable_odom_tf" ).as_bool( );
cmd_vel_timeout_ = std::chrono::milliseconds{
static_cast<int>( get_node( )->get_parameter( "cmd_vel_timeout" ).as_double( ) * 1000.0 )};
publish_limited_velocity_ = get_node( )->get_parameter( "publish_limited_velocity" ).as_bool( );
use_stamped_vel_ = get_node( )->get_parameter( "use_stamped_vel" ).as_bool( );
try
{
limiter_linear_ = SpeedLimiter(
get_node( )->get_parameter( "linear.x.has_velocity_limits" ).as_bool( ),
get_node( )->get_parameter( "linear.x.has_acceleration_limits" ).as_bool( ),
get_node( )->get_parameter( "linear.x.has_jerk_limits" ).as_bool( ),
get_node( )->get_parameter( "linear.x.min_velocity" ).as_double( ),
get_node( )->get_parameter( "linear.x.max_velocity" ).as_double( ),
get_node( )->get_parameter( "linear.x.min_acceleration" ).as_double( ),
get_node( )->get_parameter( "linear.x.max_acceleration" ).as_double( ),
get_node( )->get_parameter( "linear.x.min_jerk" ).as_double( ),
get_node( )->get_parameter( "linear.x.max_jerk" ).as_double( ) );
}
catch ( const std::runtime_error & e )
{
RCLCPP_ERROR( get_node( )->get_logger( ), "Error configuring linear speed limiter: %s", e.what( ) );
}
try
{
limiter_angular_ = SpeedLimiter(
get_node( )->get_parameter( "angular.z.has_velocity_limits" ).as_bool( ),
get_node( )->get_parameter( "angular.z.has_acceleration_limits" ).as_bool( ),
get_node( )->get_parameter( "angular.z.has_jerk_limits" ).as_bool( ),
get_node( )->get_parameter( "angular.z.min_velocity" ).as_double( ),
get_node( )->get_parameter( "angular.z.max_velocity" ).as_double( ),
get_node( )->get_parameter( "angular.z.min_acceleration" ).as_double( ),
get_node( )->get_parameter( "angular.z.max_acceleration" ).as_double( ),
get_node( )->get_parameter( "angular.z.min_jerk" ).as_double( ),
get_node( )->get_parameter( "angular.z.max_jerk" ).as_double( ) );
}
catch ( const std::runtime_error & e )
{
RCLCPP_ERROR( get_node( )->get_logger( ), "Error configuring angular speed limiter: %s", e.what( ) );
}
if ( !reset( ) )
{
return controller_interface::CallbackReturn::ERROR;
}
// left and right sides are both equal at this point
wheel_params_.wheels_per_side = left_wheel_names_.size( );
if ( publish_limited_velocity_ )
{
limited_velocity_publisher_ =
get_node( )->create_publisher<Twist>( DEFAULT_COMMAND_OUT_TOPIC, rclcpp::SystemDefaultsQoS( ) );
realtime_limited_velocity_publisher_ =
std::make_shared<realtime_tools::RealtimePublisher<Twist>>( limited_velocity_publisher_ );
}
const Twist empty_twist;
received_velocity_msg_ptr_.set( std::make_shared<Twist>( empty_twist ) );
// Fill last two commands with default constructed commands
previous_commands_.emplace( empty_twist );
previous_commands_.emplace( empty_twist );
// initialize command subscriber
if ( use_stamped_vel_ )
{
velocity_command_subscriber_ = get_node( )->create_subscription<Twist>(
DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS( ),
[this]( const std::shared_ptr<Twist> msg ) -> void {
if ( !subscriber_is_active_ )
{
RCLCPP_WARN(
get_node( )->get_logger( ), "Can't accept new commands. subscriber is inactive" );
return;
}
if ( ( msg->header.stamp.sec == 0 ) && ( msg->header.stamp.nanosec == 0 ) )
{
RCLCPP_WARN_ONCE(
get_node( )->get_logger( ),
"Received TwistStamped with zero timestamp, setting it to current "
"time, this message will only be shown once" );
msg->header.stamp = get_node( )->get_clock( )->now( );
}
received_velocity_msg_ptr_.set( std::move( msg ) );
} );
}
else
{
velocity_command_unstamped_subscriber_ =
get_node( )->create_subscription<geometry_msgs::msg::Twist>(
DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS( ),
[this]( const std::shared_ptr<geometry_msgs::msg::Twist> msg ) -> void {
if ( !subscriber_is_active_ )
{
RCLCPP_WARN(
get_node( )->get_logger( ), "Can't accept new commands. subscriber is inactive" );
return;
}
// Write fake header in the stored stamped command
std::shared_ptr<Twist> twist_stamped;
received_velocity_msg_ptr_.get( twist_stamped );
twist_stamped->twist = *msg;
twist_stamped->header.stamp = get_node( )->get_clock( )->now( );
} );
}
// initialize odometry publisher and messasge
odometry_publisher_ = get_node( )->create_publisher<nav_msgs::msg::Odometry>(
DEFAULT_ODOMETRY_TOPIC, rclcpp::SystemDefaultsQoS( ) );
realtime_odometry_publisher_ =
std::make_shared<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>(
odometry_publisher_ );
auto & odometry_message = realtime_odometry_publisher_->msg_;
odometry_message.header.frame_id = odom_params_.odom_frame_id;
odometry_message.child_frame_id = odom_params_.base_frame_id;
// limit the publication on the topics /odom and /tf
publish_rate_ = get_node( )->get_parameter( "publish_rate" ).as_double( );
publish_period_ = rclcpp::Duration::from_seconds( 1.0 / publish_rate_ );
previous_publish_timestamp_ = get_node( )->get_clock( )->now( );
// initialize odom values zeros
odometry_message.twist =
geometry_msgs::msg::TwistWithCovariance( rosidl_runtime_cpp::MessageInitialization::ALL );
constexpr size_t NUM_DIMENSIONS = 6;
for ( size_t index = 0; index < 6; ++index )
{
// 0, 7, 14, 21, 28, 35
const size_t diagonal_index = NUM_DIMENSIONS * index + index;
odometry_message.pose.covariance[diagonal_index] = odom_params_.pose_covariance_diagonal[index];
odometry_message.twist.covariance[diagonal_index] =
odom_params_.twist_covariance_diagonal[index];
}
// initialize transform publisher and message
odometry_transform_publisher_ = get_node( )->create_publisher<tf2_msgs::msg::TFMessage>(
DEFAULT_TRANSFORM_TOPIC, rclcpp::SystemDefaultsQoS( ) );
realtime_odometry_transform_publisher_ =
std::make_shared<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>>(
odometry_transform_publisher_ );
// keeping track of odom and base_link transforms only
auto & odometry_transform_message = realtime_odometry_transform_publisher_->msg_;
odometry_transform_message.transforms.resize( 1 );
odometry_transform_message.transforms.front( ).header.frame_id = odom_params_.odom_frame_id;
odometry_transform_message.transforms.front( ).child_frame_id = odom_params_.base_frame_id;
previous_update_timestamp_ = get_node( )->get_clock( )->now( );
return controller_interface::CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn DiffDriveController::on_activate(
const rclcpp_lifecycle::State & )
{
const auto left_result =
configure_side( "left", left_wheel_names_, registered_left_wheel_handles_ );
const auto right_result =
configure_side( "right", right_wheel_names_, registered_right_wheel_handles_ );
if (
left_result == controller_interface::CallbackReturn::ERROR ||
right_result == controller_interface::CallbackReturn::ERROR )
{
return controller_interface::CallbackReturn::ERROR;
}
if ( registered_left_wheel_handles_.empty( ) || registered_right_wheel_handles_.empty( ) )
{
RCLCPP_ERROR(
get_node( )->get_logger( ),
"Either left wheel interfaces, right wheel interfaces are non existent" );
return controller_interface::CallbackReturn::ERROR;
}
is_halted = false;
subscriber_is_active_ = true;
RCLCPP_DEBUG( get_node( )->get_logger( ), "Subscriber and publisher are now active." );
return controller_interface::CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn DiffDriveController::on_deactivate(
const rclcpp_lifecycle::State & )
{
subscriber_is_active_ = false;
return controller_interface::CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn DiffDriveController::on_cleanup(
const rclcpp_lifecycle::State & )
{
if ( !reset( ) )
{
return controller_interface::CallbackReturn::ERROR;
}
received_velocity_msg_ptr_.set( std::make_shared<Twist>( ) );
return controller_interface::CallbackReturn::SUCCESS;
}
controller_interface::CallbackReturn DiffDriveController::on_error( const rclcpp_lifecycle::State & )
{
if ( !reset( ) )
{
return controller_interface::CallbackReturn::ERROR;
}
return controller_interface::CallbackReturn::SUCCESS;
}
bool DiffDriveController::reset( )
{
odometry_.resetOdometry( );
// release the old queue
std::queue<Twist> empty;
std::swap( previous_commands_, empty );
registered_left_wheel_handles_.clear( );
registered_right_wheel_handles_.clear( );
subscriber_is_active_ = false;
velocity_command_subscriber_.reset( );
velocity_command_unstamped_subscriber_.reset( );
received_velocity_msg_ptr_.set( nullptr );
is_halted = false;
return true;
}
controller_interface::CallbackReturn DiffDriveController::on_shutdown(
const rclcpp_lifecycle::State & )
{
return controller_interface::CallbackReturn::SUCCESS;
}
void DiffDriveController::halt( )
{
const auto halt_wheels = []( auto & wheel_handles ) {
for ( const auto & wheel_handle : wheel_handles )
{
wheel_handle.velocity.get( ).set_value( 0.0 );
}
};
halt_wheels( registered_left_wheel_handles_ );
halt_wheels( registered_right_wheel_handles_ );
}
controller_interface::CallbackReturn DiffDriveController::configure_side(
const std::string & side, const std::vector<std::string> & wheel_names,
std::vector<WheelHandle> & registered_handles )
{
auto logger = get_node( )->get_logger( );
if ( wheel_names.empty( ) )
{
RCLCPP_ERROR( logger, "No '%s' wheel names specified", side.c_str( ) );
return controller_interface::CallbackReturn::ERROR;
}
// register handles
registered_handles.reserve( wheel_names.size( ) );
for ( const auto & wheel_name : wheel_names )
{
const auto interface_name = feedback_type( );
const auto state_handle = std::find_if(
state_interfaces_.cbegin( ), state_interfaces_.cend( ),
[&wheel_name, &interface_name]( const auto & interface ) {
return interface.get_prefix_name( ) == wheel_name &&
interface.get_interface_name( ) == interface_name;
} );
if ( state_handle == state_interfaces_.cend( ) )
{
RCLCPP_ERROR( logger, "Unable to obtain joint state handle for %s", wheel_name.c_str( ) );
return controller_interface::CallbackReturn::ERROR;
}
const auto command_handle = std::find_if(
command_interfaces_.begin( ), command_interfaces_.end( ),
[&wheel_name]( const auto & interface ) {
return interface.get_prefix_name( ) == wheel_name &&
interface.get_interface_name( ) == HW_IF_VELOCITY;
} );
if ( command_handle == command_interfaces_.end( ) )
{
RCLCPP_ERROR( logger, "Unable to obtain joint command handle for %s", wheel_name.c_str( ) );
return controller_interface::CallbackReturn::ERROR;
}
registered_handles.emplace_back(
WheelHandle{std::ref( *state_handle ), std::ref( *command_handle )} );
}
return controller_interface::CallbackReturn::SUCCESS;
}
} // namespace diff_drive_controller
#include "class_loader/register_macro.hpp"
CLASS_LOADER_REGISTER_CLASS(
diff_drive_controller::DiffDriveController, controller_interface::ControllerInterface )
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*
* Author: Enrique Fernández
*/
#include "diff_drive_controller/odometry.hpp"
namespace diff_drive_controller
{
23 Odometry::Odometry( size_t velocity_rolling_window_size )
: timestamp_( 0.0 ),
x_( 0.0 ),
y_( 0.0 ),
heading_( 0.0 ),
linear_( 0.0 ),
angular_( 0.0 ),
wheel_separation_( 0.0 ),
left_wheel_radius_( 0.0 ),
right_wheel_radius_( 0.0 ),
left_wheel_old_pos_( 0.0 ),
right_wheel_old_pos_( 0.0 ),
velocity_rolling_window_size_( velocity_rolling_window_size ),
linear_accumulator_( velocity_rolling_window_size ),
angular_accumulator_( velocity_rolling_window_size )
{
}
41 void Odometry::init( const rclcpp::Time & time )
{
// Reset accumulators and timestamp:
resetAccumulators( );
timestamp_ = time;
}
48 bool Odometry::update( double left_pos, double right_pos, const rclcpp::Time & time )
{
// We cannot estimate the speed with very small time intervals:
const double dt = time.seconds( ) - timestamp_.seconds( );
if ( dt < 0.0001 )
{
return false; // Interval too small to integrate with
}
// Get current wheel joint positions:
const double left_wheel_cur_pos = left_pos * left_wheel_radius_;
const double right_wheel_cur_pos = right_pos * right_wheel_radius_;
// Estimate velocity of wheels using old and current position:
const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_;
const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_;
// Update old position with current:
left_wheel_old_pos_ = left_wheel_cur_pos;
right_wheel_old_pos_ = right_wheel_cur_pos;
updateFromVelocity( left_wheel_est_vel, right_wheel_est_vel, time );
return true;
}
74 bool Odometry::updateFromVelocity( double left_vel, double right_vel, const rclcpp::Time & time )
{
const double dt = time.seconds( ) - timestamp_.seconds( );
// Compute linear and angular diff:
const double linear = ( left_vel + right_vel ) * 0.5;
// Now there is a bug about scout angular velocity
const double angular = ( right_vel - left_vel ) / wheel_separation_;
// Integrate odometry:
integrateExact( linear, angular );
timestamp_ = time;
// Estimate speeds using a rolling mean to filter them out:
linear_accumulator_.accumulate( linear / dt );
angular_accumulator_.accumulate( angular / dt );
linear_ = linear_accumulator_.getRollingMean( );
angular_ = angular_accumulator_.getRollingMean( );
return true;
}
98 void Odometry::updateOpenLoop( double linear, double angular, const rclcpp::Time & time )
{
/// Save last linear and angular velocity:
linear_ = linear;
angular_ = angular;
/// Integrate odometry:
const double dt = time.seconds( ) - timestamp_.seconds( );
timestamp_ = time;
integrateExact( linear * dt, angular * dt );
}
110 void Odometry::resetOdometry( )
{
x_ = 0.0;
y_ = 0.0;
heading_ = 0.0;
}
117 void Odometry::setWheelParams(
double wheel_separation, double left_wheel_radius, double right_wheel_radius )
{
wheel_separation_ = wheel_separation;
left_wheel_radius_ = left_wheel_radius;
right_wheel_radius_ = right_wheel_radius;
}
125 void Odometry::setVelocityRollingWindowSize( size_t velocity_rolling_window_size )
{
velocity_rolling_window_size_ = velocity_rolling_window_size;
resetAccumulators( );
}
132 void Odometry::integrateRungeKutta2( double linear, double angular )
{
const double direction = heading_ + angular * 0.5;
/// Runge-Kutta 2nd order integration:
x_ += linear * cos( direction );
y_ += linear * sin( direction );
heading_ += angular;
}
142 void Odometry::integrateExact( double linear, double angular )
{
if ( fabs( angular ) < 1e-6 )
{
integrateRungeKutta2( linear, angular );
}
else
{
/// Exact integration ( should solve problems when angular is zero ):
const double heading_old = heading_;
const double r = linear / angular;
heading_ += angular;
x_ += r * ( sin( heading_ ) - sin( heading_old ) );
y_ += -r * ( cos( heading_ ) - cos( heading_old ) );
}
}
159 void Odometry::resetAccumulators( )
{
linear_accumulator_ = RollingMeanAccumulator( velocity_rolling_window_size_ );
angular_accumulator_ = RollingMeanAccumulator( velocity_rolling_window_size_ );
}
} // namespace diff_drive_controller
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*
* Author: Enrique Fernández
*/
#include <algorithm>
#include <stdexcept>
#include "diff_drive_controller/speed_limiter.hpp"
#include "rcppmath/clamp.hpp"
namespace diff_drive_controller
{
27 SpeedLimiter::SpeedLimiter(
28 bool has_velocity_limits, bool has_acceleration_limits, bool has_jerk_limits, double min_velocity,
double max_velocity, double min_acceleration, double max_acceleration, double min_jerk,
double max_jerk )
: has_velocity_limits_( has_velocity_limits ),
has_acceleration_limits_( has_acceleration_limits ),
has_jerk_limits_( has_jerk_limits ),
min_velocity_( min_velocity ),
max_velocity_( max_velocity ),
min_acceleration_( min_acceleration ),
max_acceleration_( max_acceleration ),
min_jerk_( min_jerk ),
max_jerk_( max_jerk )
{
// Check if limits are valid, max must be specified, min defaults to -max if unspecified
if ( has_velocity_limits_ )
{
if ( std::isnan( max_velocity_ ) )
{
throw std::runtime_error( "Cannot apply velocity limits if max_velocity is not specified" );
}
if ( std::isnan( min_velocity_ ) )
{
min_velocity_ = -max_velocity_;
}
}
if ( has_acceleration_limits_ )
{
if ( std::isnan( max_acceleration_ ) )
{
throw std::runtime_error(
"Cannot apply acceleration limits if max_acceleration is not specified" );
}
if ( std::isnan( min_acceleration_ ) )
{
min_acceleration_ = -max_acceleration_;
}
}
if ( has_jerk_limits_ )
{
if ( std::isnan( max_jerk_ ) )
{
throw std::runtime_error( "Cannot apply jerk limits if max_jerk is not specified" );
}
if ( std::isnan( min_jerk_ ) )
{
min_jerk_ = -max_jerk_;
}
}
}
78 double SpeedLimiter::limit( double & v, double v0, double v1, double dt )
{
const double tmp = v;
limit_jerk( v, v0, v1, dt );
limit_acceleration( v, v0, dt );
limit_velocity( v );
return tmp != 0.0 ? v / tmp : 1.0;
}
89 double SpeedLimiter::limit_velocity( double & v )
{
const double tmp = v;
if ( has_velocity_limits_ )
{
v = rcppmath::clamp( v, min_velocity_, max_velocity_ );
}
return tmp != 0.0 ? v / tmp : 1.0;
}
101 double SpeedLimiter::limit_acceleration( double & v, double v0, double dt )
{
const double tmp = v;
if ( has_acceleration_limits_ )
{
const double dv_min = min_acceleration_ * dt;
const double dv_max = max_acceleration_ * dt;
const double dv = rcppmath::clamp( v - v0, dv_min, dv_max );
v = v0 + dv;
}
return tmp != 0.0 ? v / tmp : 1.0;
}
118 double SpeedLimiter::limit_jerk( double & v, double v0, double v1, double dt )
{
const double tmp = v;
if ( has_jerk_limits_ )
{
const double dv = v - v0;
const double dv0 = v0 - v1;
const double dt2 = 2. * dt * dt;
const double da_min = min_jerk_ * dt2;
const double da_max = max_jerk_ * dt2;
const double da = rcppmath::clamp( dv - dv0, da_min, da_max );
v = v0 + dv0 + da;
}
return tmp != 0.0 ? v / tmp : 1.0;
}
} // namespace diff_drive_controller
// Copyright 2020 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gmock/gmock.h>
#include <array>
#include <memory>
#include <string>
#include <thread>
#include <utility>
#include <vector>
#include "diff_drive_controller/diff_drive_controller.hpp"
#include "hardware_interface/loaned_command_interface.hpp"
#include "hardware_interface/loaned_state_interface.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/rclcpp.hpp"
using CallbackReturn = controller_interface::CallbackReturn;
using hardware_interface::HW_IF_POSITION;
using hardware_interface::HW_IF_VELOCITY;
using hardware_interface::LoanedCommandInterface;
using hardware_interface::LoanedStateInterface;
using lifecycle_msgs::msg::State;
using testing::SizeIs;
39 class TestableDiffDriveController : public diff_drive_controller::DiffDriveController
{
public:
using DiffDriveController::DiffDriveController;
std::shared_ptr<geometry_msgs::msg::TwistStamped> getLastReceivedTwist( )
{
std::shared_ptr<geometry_msgs::msg::TwistStamped> ret;
received_velocity_msg_ptr_.get( ret );
return ret;
}
/**
* @brief wait_for_twist block until a new twist is received.
* Requires that the executor is not spinned elsewhere between the
* message publication and the call to this function
*
* @return true if new twist msg was received, false if timeout
*/
bool wait_for_twist(
rclcpp::Executor & executor,
const std::chrono::milliseconds & timeout = std::chrono::milliseconds( 500 ) )
{
rclcpp::WaitSet wait_set;
62 wait_set.add_subscription( velocity_command_subscriber_ );
64 if ( wait_set.wait( timeout ).kind( ) == rclcpp::WaitResultKind::Ready )
{
executor.spin_some( );
return true;
}
return false;
}
};
class TestDiffDriveController : public ::testing::Test
{
protected:
static void SetUpTestCase( ) { rclcpp::init( 0, nullptr ); }
void SetUp( ) override
{
controller_ = std::make_unique<TestableDiffDriveController>( );
pub_node = std::make_shared<rclcpp::Node>( "velocity_publisher" );
velocity_publisher = pub_node->create_publisher<geometry_msgs::msg::TwistStamped>(
controller_name + "/cmd_vel", rclcpp::SystemDefaultsQoS( ) );
}
static void TearDownTestCase( ) { rclcpp::shutdown( ); }
/// Publish velocity msgs
/**
* linear - magnitude of the linear command in the geometry_msgs::twist message
* angular - the magnitude of the angular command in geometry_msgs::twist message
*/
void publish( double linear, double angular )
{
int wait_count = 0;
auto topic = velocity_publisher->get_topic_name( );
while ( pub_node->count_subscribers( topic ) == 0 )
{
if ( wait_count >= 5 )
{
auto error_msg = std::string( "publishing to " ) + topic + " but no node subscribes to it";
throw std::runtime_error( error_msg );
}
std::this_thread::sleep_for( std::chrono::milliseconds( 100 ) );
++wait_count;
}
geometry_msgs::msg::TwistStamped velocity_message;
velocity_message.header.stamp = pub_node->get_clock( )->now( );
velocity_message.twist.linear.x = linear;
velocity_message.twist.angular.z = angular;
velocity_publisher->publish( velocity_message );
}
/// \brief wait for the subscriber and publisher to completely setup
void waitForSetup( )
{
constexpr std::chrono::seconds TIMEOUT{2};
auto clock = pub_node->get_clock( );
auto start = clock->now( );
while ( velocity_publisher->get_subscription_count( ) <= 0 )
{
if ( ( clock->now( ) - start ) > TIMEOUT )
{
FAIL( );
}
rclcpp::spin_some( pub_node );
}
}
void assignResourcesPosFeedback( )
{
std::vector<LoanedStateInterface> state_ifs;
state_ifs.emplace_back( left_wheel_pos_state_ );
state_ifs.emplace_back( right_wheel_pos_state_ );
std::vector<LoanedCommandInterface> command_ifs;
command_ifs.emplace_back( left_wheel_vel_cmd_ );
command_ifs.emplace_back( right_wheel_vel_cmd_ );
controller_->assign_interfaces( std::move( command_ifs ), std::move( state_ifs ) );
}
void assignResourcesVelFeedback( )
{
std::vector<LoanedStateInterface> state_ifs;
state_ifs.emplace_back( left_wheel_vel_state_ );
state_ifs.emplace_back( right_wheel_vel_state_ );
std::vector<LoanedCommandInterface> command_ifs;
command_ifs.emplace_back( left_wheel_vel_cmd_ );
command_ifs.emplace_back( right_wheel_vel_cmd_ );
controller_->assign_interfaces( std::move( command_ifs ), std::move( state_ifs ) );
}
const std::string controller_name = "test_diff_drive_controller";
std::unique_ptr<TestableDiffDriveController> controller_;
const std::vector<std::string> left_wheel_names = {"left_wheel_joint"};
const std::vector<std::string> right_wheel_names = {"right_wheel_joint"};
std::vector<double> position_values_ = {0.1, 0.2};
std::vector<double> velocity_values_ = {0.01, 0.02};
hardware_interface::StateInterface left_wheel_pos_state_{
left_wheel_names[0], HW_IF_POSITION, &position_values_[0]};
hardware_interface::StateInterface right_wheel_pos_state_{
right_wheel_names[0], HW_IF_POSITION, &position_values_[1]};
hardware_interface::StateInterface left_wheel_vel_state_{
left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]};
hardware_interface::StateInterface right_wheel_vel_state_{
right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]};
hardware_interface::CommandInterface left_wheel_vel_cmd_{
left_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[0]};
hardware_interface::CommandInterface right_wheel_vel_cmd_{
right_wheel_names[0], HW_IF_VELOCITY, &velocity_values_[1]};
rclcpp::Node::SharedPtr pub_node;
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr velocity_publisher;
};
TEST_F( TestDiffDriveController, configure_fails_without_parameters )
{
const auto ret = controller_->init( controller_name );
ASSERT_EQ( ret, controller_interface::return_type::OK );
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::ERROR );
}
TEST_F( TestDiffDriveController, configure_fails_with_only_left_or_only_right_side_defined )
{
const auto ret = controller_->init( controller_name );
ASSERT_EQ( ret, controller_interface::return_type::OK );
controller_->get_node( )->set_parameter(
rclcpp::Parameter( "left_wheel_names", rclcpp::ParameterValue( left_wheel_names ) ) );
controller_->get_node( )->set_parameter(
rclcpp::Parameter( "right_wheel_names", rclcpp::ParameterValue( std::vector<std::string>( ) ) ) );
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::ERROR );
controller_->get_node( )->set_parameter(
rclcpp::Parameter( "left_wheel_names", rclcpp::ParameterValue( std::vector<std::string>( ) ) ) );
controller_->get_node( )->set_parameter(
rclcpp::Parameter( "right_wheel_names", rclcpp::ParameterValue( right_wheel_names ) ) );
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::ERROR );
}
TEST_F( TestDiffDriveController, configure_fails_with_mismatching_wheel_side_size )
{
const auto ret = controller_->init( controller_name );
ASSERT_EQ( ret, controller_interface::return_type::OK );
controller_->get_node( )->set_parameter(
rclcpp::Parameter( "left_wheel_names", rclcpp::ParameterValue( left_wheel_names ) ) );
auto extended_right_wheel_names = right_wheel_names;
extended_right_wheel_names.push_back( "extra_wheel" );
controller_->get_node( )->set_parameter(
rclcpp::Parameter( "right_wheel_names", rclcpp::ParameterValue( extended_right_wheel_names ) ) );
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::ERROR );
}
TEST_F( TestDiffDriveController, configure_succeeds_when_wheels_are_specified )
{
const auto ret = controller_->init( controller_name );
ASSERT_EQ( ret, controller_interface::return_type::OK );
controller_->get_node( )->set_parameter(
rclcpp::Parameter( "left_wheel_names", rclcpp::ParameterValue( left_wheel_names ) ) );
controller_->get_node( )->set_parameter(
rclcpp::Parameter( "right_wheel_names", rclcpp::ParameterValue( right_wheel_names ) ) );
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
ASSERT_THAT(
controller_->state_interface_configuration( ).names,
SizeIs( left_wheel_names.size( ) + right_wheel_names.size( ) ) );
ASSERT_THAT(
controller_->command_interface_configuration( ).names,
SizeIs( left_wheel_names.size( ) + right_wheel_names.size( ) ) );
}
TEST_F( TestDiffDriveController, activate_fails_without_resources_assigned )
{
const auto ret = controller_->init( controller_name );
ASSERT_EQ( ret, controller_interface::return_type::OK );
controller_->get_node( )->set_parameter(
rclcpp::Parameter( "left_wheel_names", rclcpp::ParameterValue( left_wheel_names ) ) );
controller_->get_node( )->set_parameter(
rclcpp::Parameter( "right_wheel_names", rclcpp::ParameterValue( right_wheel_names ) ) );
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
ASSERT_EQ( controller_->on_activate( rclcpp_lifecycle::State( ) ), CallbackReturn::ERROR );
}
TEST_F( TestDiffDriveController, activate_succeeds_with_pos_resources_assigned )
{
const auto ret = controller_->init( controller_name );
ASSERT_EQ( ret, controller_interface::return_type::OK );
// We implicitly test that by default position feedback is required
controller_->get_node( )->set_parameter(
rclcpp::Parameter( "left_wheel_names", rclcpp::ParameterValue( left_wheel_names ) ) );
controller_->get_node( )->set_parameter(
rclcpp::Parameter( "right_wheel_names", rclcpp::ParameterValue( right_wheel_names ) ) );
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
assignResourcesPosFeedback( );
ASSERT_EQ( controller_->on_activate( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
}
TEST_F( TestDiffDriveController, activate_succeeds_with_vel_resources_assigned )
{
const auto ret = controller_->init( controller_name );
ASSERT_EQ( ret, controller_interface::return_type::OK );
controller_->get_node( )->set_parameter(
rclcpp::Parameter( "position_feedback", rclcpp::ParameterValue( false ) ) );
controller_->get_node( )->set_parameter(
rclcpp::Parameter( "left_wheel_names", rclcpp::ParameterValue( left_wheel_names ) ) );
controller_->get_node( )->set_parameter(
rclcpp::Parameter( "right_wheel_names", rclcpp::ParameterValue( right_wheel_names ) ) );
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
assignResourcesVelFeedback( );
ASSERT_EQ( controller_->on_activate( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
}
TEST_F( TestDiffDriveController, activate_fails_with_wrong_resources_assigned_1 )
{
const auto ret = controller_->init( controller_name );
ASSERT_EQ( ret, controller_interface::return_type::OK );
controller_->get_node( )->set_parameter(
rclcpp::Parameter( "position_feedback", rclcpp::ParameterValue( false ) ) );
controller_->get_node( )->set_parameter(
rclcpp::Parameter( "left_wheel_names", rclcpp::ParameterValue( left_wheel_names ) ) );
controller_->get_node( )->set_parameter(
rclcpp::Parameter( "right_wheel_names", rclcpp::ParameterValue( right_wheel_names ) ) );
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
assignResourcesPosFeedback( );
ASSERT_EQ( controller_->on_activate( rclcpp_lifecycle::State( ) ), CallbackReturn::ERROR );
}
TEST_F( TestDiffDriveController, activate_fails_with_wrong_resources_assigned_2 )
{
const auto ret = controller_->init( controller_name );
ASSERT_EQ( ret, controller_interface::return_type::OK );
controller_->get_node( )->set_parameter(
rclcpp::Parameter( "position_feedback", rclcpp::ParameterValue( true ) ) );
controller_->get_node( )->set_parameter(
rclcpp::Parameter( "left_wheel_names", rclcpp::ParameterValue( left_wheel_names ) ) );
controller_->get_node( )->set_parameter(
rclcpp::Parameter( "right_wheel_names", rclcpp::ParameterValue( right_wheel_names ) ) );
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
assignResourcesVelFeedback( );
ASSERT_EQ( controller_->on_activate( rclcpp_lifecycle::State( ) ), CallbackReturn::ERROR );
}
TEST_F( TestDiffDriveController, cleanup )
{
const auto ret = controller_->init( controller_name );
ASSERT_EQ( ret, controller_interface::return_type::OK );
controller_->get_node( )->set_parameter(
rclcpp::Parameter( "left_wheel_names", rclcpp::ParameterValue( left_wheel_names ) ) );
controller_->get_node( )->set_parameter(
rclcpp::Parameter( "right_wheel_names", rclcpp::ParameterValue( right_wheel_names ) ) );
controller_->get_node( )->set_parameter( rclcpp::Parameter( "wheel_separation", 0.4 ) );
controller_->get_node( )->set_parameter( rclcpp::Parameter( "wheel_radius", 0.1 ) );
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node( controller_->get_node( )->get_node_base_interface( ) );
auto state = controller_->get_node( )->configure( );
ASSERT_EQ( State::PRIMARY_STATE_INACTIVE, state.id( ) );
assignResourcesPosFeedback( );
state = controller_->get_node( )->activate( );
ASSERT_EQ( State::PRIMARY_STATE_ACTIVE, state.id( ) );
waitForSetup( );
// send msg
const double linear = 1.0;
const double angular = 1.0;
publish( linear, angular );
controller_->wait_for_twist( executor );
ASSERT_EQ(
controller_->update( rclcpp::Time( 0, 0, RCL_ROS_TIME ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
state = controller_->get_node( )->deactivate( );
ASSERT_EQ( State::PRIMARY_STATE_INACTIVE, state.id( ) );
ASSERT_EQ(
controller_->update( rclcpp::Time( 0, 0, RCL_ROS_TIME ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
state = controller_->get_node( )->cleanup( );
ASSERT_EQ( State::PRIMARY_STATE_UNCONFIGURED, state.id( ) );
// should be stopped
EXPECT_EQ( 0.0, left_wheel_vel_cmd_.get_value( ) );
EXPECT_EQ( 0.0, right_wheel_vel_cmd_.get_value( ) );
executor.cancel( );
}
TEST_F( TestDiffDriveController, correct_initialization_using_parameters )
{
const auto ret = controller_->init( controller_name );
ASSERT_EQ( ret, controller_interface::return_type::OK );
controller_->get_node( )->set_parameter(
rclcpp::Parameter( "left_wheel_names", rclcpp::ParameterValue( left_wheel_names ) ) );
controller_->get_node( )->set_parameter(
rclcpp::Parameter( "right_wheel_names", rclcpp::ParameterValue( right_wheel_names ) ) );
controller_->get_node( )->set_parameter( rclcpp::Parameter( "wheel_separation", 0.4 ) );
controller_->get_node( )->set_parameter( rclcpp::Parameter( "wheel_radius", 1.0 ) );
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node( controller_->get_node( )->get_node_base_interface( ) );
auto state = controller_->get_node( )->configure( );
assignResourcesPosFeedback( );
ASSERT_EQ( State::PRIMARY_STATE_INACTIVE, state.id( ) );
EXPECT_EQ( 0.01, left_wheel_vel_cmd_.get_value( ) );
EXPECT_EQ( 0.02, right_wheel_vel_cmd_.get_value( ) );
state = controller_->get_node( )->activate( );
ASSERT_EQ( State::PRIMARY_STATE_ACTIVE, state.id( ) );
// send msg
const double linear = 1.0;
const double angular = 0.0;
publish( linear, angular );
// wait for msg is be published to the system
ASSERT_TRUE( controller_->wait_for_twist( executor ) );
ASSERT_EQ(
controller_->update( rclcpp::Time( 0, 0, RCL_ROS_TIME ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
EXPECT_EQ( 1.0, left_wheel_vel_cmd_.get_value( ) );
EXPECT_EQ( 1.0, right_wheel_vel_cmd_.get_value( ) );
// deactivated
// wait so controller process the second point when deactivated
std::this_thread::sleep_for( std::chrono::milliseconds( 500 ) );
state = controller_->get_node( )->deactivate( );
ASSERT_EQ( state.id( ), State::PRIMARY_STATE_INACTIVE );
ASSERT_EQ(
controller_->update( rclcpp::Time( 0, 0, RCL_ROS_TIME ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
EXPECT_EQ( 0.0, left_wheel_vel_cmd_.get_value( ) ) << "Wheels are halted on deactivate( )";
EXPECT_EQ( 0.0, right_wheel_vel_cmd_.get_value( ) ) << "Wheels are halted on deactivate( )";
// cleanup
state = controller_->get_node( )->cleanup( );
ASSERT_EQ( State::PRIMARY_STATE_UNCONFIGURED, state.id( ) );
EXPECT_EQ( 0.0, left_wheel_vel_cmd_.get_value( ) );
EXPECT_EQ( 0.0, right_wheel_vel_cmd_.get_value( ) );
state = controller_->get_node( )->configure( );
ASSERT_EQ( State::PRIMARY_STATE_INACTIVE, state.id( ) );
executor.cancel( );
}
1 // Copyright 2020 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gmock/gmock.h>
#include <memory>
#include "controller_manager/controller_manager.hpp"
#include "rclcpp/utilities.hpp"
#include "ros2_control_test_assets/descriptions.hpp"
22 TEST( TestLoadDiffDriveController, load_controller )
{
rclcpp::init( 0, nullptr );
std::shared_ptr<rclcpp::Executor> executor =
std::make_shared<rclcpp::executors::SingleThreadedExecutor>( );
controller_manager::ControllerManager cm(
std::make_unique<hardware_interface::ResourceManager>( ros2_control_test_assets::diffbot_urdf ),
executor, "test_controller_manager" );
ASSERT_NO_THROW(
cm.load_controller( "test_diff_drive_controller", "diff_drive_controller/DiffDriveController" ) );
rclcpp::shutdown( );
}
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_
#define EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_
#include <string>
#include "effort_controllers/visibility_control.h"
#include "forward_command_controller/forward_command_controller.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
namespace effort_controllers
{
/**
* \brief Forward command controller for a set of effort controlled joints ( linear or angular ).
*
* This class forwards the commanded efforts down to a set of joints.
*
* \param joints Names of the joints to control.
*
* Subscribes to:
* - \b command ( std_msgs::msg::Float64MultiArray ) : The effort commands to apply.
*/
36 class JointGroupEffortController : public forward_command_controller::ForwardCommandController
{
public:
EFFORT_CONTROLLERS_PUBLIC
40 JointGroupEffortController( );
EFFORT_CONTROLLERS_PUBLIC
controller_interface::CallbackReturn on_init( ) override;
EFFORT_CONTROLLERS_PUBLIC
controller_interface::CallbackReturn on_deactivate(
const rclcpp_lifecycle::State & previous_state ) override;
};
} // namespace effort_controllers
#endif // EFFORT_CONTROLLERS__JOINT_GROUP_EFFORT_CONTROLLER_HPP_
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/* This header must be included by all rclcpp headers which declare symbols
* which are defined in the rclcpp library. When not building the rclcpp
* library, i.e. when using the headers in other package's code, the contents
* of this header change the visibility of certain symbols which the rclcpp
* library cannot have, but the consuming code must have inorder to link.
*/
#ifndef EFFORT_CONTROLLERS__VISIBILITY_CONTROL_H_
#define EFFORT_CONTROLLERS__VISIBILITY_CONTROL_H_
// This logic was borrowed ( then namespaced ) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define EFFORT_CONTROLLERS_EXPORT __attribute__( ( dllexport ) )
#define EFFORT_CONTROLLERS_IMPORT __attribute__( ( dllimport ) )
#else
#define EFFORT_CONTROLLERS_EXPORT __declspec( dllexport )
#define EFFORT_CONTROLLERS_IMPORT __declspec( dllimport )
#endif
#ifdef EFFORT_CONTROLLERS_BUILDING_DLL
#define EFFORT_CONTROLLERS_PUBLIC EFFORT_CONTROLLERS_EXPORT
#else
#define EFFORT_CONTROLLERS_PUBLIC EFFORT_CONTROLLERS_IMPORT
#endif
#define EFFORT_CONTROLLERS_PUBLIC_TYPE EFFORT_CONTROLLERS_PUBLIC
#define EFFORT_CONTROLLERS_LOCAL
#else
#define EFFORT_CONTROLLERS_EXPORT __attribute__( ( visibility( "default" ) ) )
#define EFFORT_CONTROLLERS_IMPORT
#if __GNUC__ >= 4
#define EFFORT_CONTROLLERS_PUBLIC __attribute__( ( visibility( "default" ) ) )
#define EFFORT_CONTROLLERS_LOCAL __attribute__( ( visibility( "hidden" ) ) )
#else
#define EFFORT_CONTROLLERS_PUBLIC
#define EFFORT_CONTROLLERS_LOCAL
#endif
#define EFFORT_CONTROLLERS_PUBLIC_TYPE
#endif
#endif // EFFORT_CONTROLLERS__VISIBILITY_CONTROL_H_
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include "controller_interface/controller_interface.hpp"
#include "effort_controllers/joint_group_effort_controller.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/parameter.hpp"
namespace effort_controllers
{
25 JointGroupEffortController::JointGroupEffortController( )
: forward_command_controller::ForwardCommandController( )
{
interface_name_ = hardware_interface::HW_IF_EFFORT;
}
31 controller_interface::CallbackReturn JointGroupEffortController::on_init( )
{
auto ret = forward_command_controller::ForwardCommandController::on_init( );
if ( ret != controller_interface::CallbackReturn::SUCCESS )
{
return ret;
}
try
{
// Explicitly set the interface parameter declared by the forward_command_controller
// to match the value set in the JointGroupEffortController constructor.
get_node( )->set_parameter(
rclcpp::Parameter( "interface_name", hardware_interface::HW_IF_EFFORT ) );
}
catch ( const std::exception & e )
{
fprintf( stderr, "Exception thrown during init stage with message: %s \n", e.what( ) );
return controller_interface::CallbackReturn::ERROR;
}
return controller_interface::CallbackReturn::SUCCESS;
}
55 controller_interface::CallbackReturn JointGroupEffortController::on_deactivate(
56 const rclcpp_lifecycle::State & previous_state )
{
auto ret = ForwardCommandController::on_deactivate( previous_state );
// stop all joints
for ( auto & command_interface : command_interfaces_ )
{
command_interface.set_value( 0.0 );
}
return ret;
}
} // namespace effort_controllers
#include "pluginlib/class_list_macros.hpp"
73 PLUGINLIB_EXPORT_CLASS(
effort_controllers::JointGroupEffortController, controller_interface::ControllerInterface )
1 // Copyright 2020 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <stddef.h>
#include <functional>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "hardware_interface/loaned_command_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/utilities.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "test_joint_group_effort_controller.hpp"
using CallbackReturn = controller_interface::CallbackReturn;
using hardware_interface::LoanedCommandInterface;
namespace
{
35 rclcpp::WaitResultKind wait_for( rclcpp::SubscriptionBase::SharedPtr subscription )
{
rclcpp::WaitSet wait_set;
wait_set.add_subscription( subscription );
const auto timeout = std::chrono::seconds( 10 );
return wait_set.wait( timeout ).kind( );
}
} // namespace
44 void JointGroupEffortControllerTest::SetUpTestCase( ) { rclcpp::init( 0, nullptr ); }
46 void JointGroupEffortControllerTest::TearDownTestCase( ) { rclcpp::shutdown( ); }
48 void JointGroupEffortControllerTest::SetUp( )
{
controller_ = std::make_unique<FriendJointGroupEffortController>( );
}
53 void JointGroupEffortControllerTest::TearDown( ) { controller_.reset( nullptr ); }
55 void JointGroupEffortControllerTest::SetUpController( )
{
const auto result = controller_->init( "test_joint_group_effort_controller" );
ASSERT_EQ( result, controller_interface::return_type::OK );
std::vector<LoanedCommandInterface> command_ifs;
command_ifs.emplace_back( joint_1_cmd_ );
command_ifs.emplace_back( joint_2_cmd_ );
command_ifs.emplace_back( joint_3_cmd_ );
controller_->assign_interfaces( std::move( command_ifs ), {} );
}
67 TEST_F( JointGroupEffortControllerTest, JointsParameterNotSet )
{
SetUpController( );
// configure failed, 'joints' parameter not set
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::ERROR );
}
75 TEST_F( JointGroupEffortControllerTest, JointsParameterIsEmpty )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", std::vector<std::string>( )} );
// configure failed, 'joints' is empty
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::ERROR );
}
84 TEST_F( JointGroupEffortControllerTest, ConfigureAndActivateParamsSuccess )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", joint_names_} );
// configure successful
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
ASSERT_EQ( controller_->on_activate( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
}
94 TEST_F( JointGroupEffortControllerTest, ActivateWithWrongJointsNamesFails )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", std::vector<std::string>{"joint1", "joint4"}} );
// activate failed, 'joint4' is not a valid joint name for the hardware
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
ASSERT_EQ( controller_->on_activate( rclcpp_lifecycle::State( ) ), CallbackReturn::ERROR );
controller_->get_node( )->set_parameter( {"joints", std::vector<std::string>{"joint1", "joint2"}} );
// activate failed, 'acceleration' is not a registered interface for `joint1`
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
ASSERT_EQ( controller_->on_activate( rclcpp_lifecycle::State( ) ), CallbackReturn::ERROR );
}
110 TEST_F( JointGroupEffortControllerTest, CommandSuccessTest )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", joint_names_} );
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
// update successful though no command has been send yet
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
// check joint commands are still the default ones
ASSERT_EQ( joint_1_cmd_.get_value( ), 1.1 );
ASSERT_EQ( joint_2_cmd_.get_value( ), 2.1 );
ASSERT_EQ( joint_3_cmd_.get_value( ), 3.1 );
// send command
auto command_ptr = std::make_shared<forward_command_controller::CmdType>( );
command_ptr->data = {10.0, 20.0, 30.0};
controller_->rt_command_ptr_.writeFromNonRT( command_ptr );
// update successful, command received
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
// check joint commands have been modified
ASSERT_EQ( joint_1_cmd_.get_value( ), 10.0 );
ASSERT_EQ( joint_2_cmd_.get_value( ), 20.0 );
ASSERT_EQ( joint_3_cmd_.get_value( ), 30.0 );
}
142 TEST_F( JointGroupEffortControllerTest, WrongCommandCheckTest )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", joint_names_} );
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
// send command with wrong number of joints
auto command_ptr = std::make_shared<forward_command_controller::CmdType>( );
command_ptr->data = {10.0, 20.0};
controller_->rt_command_ptr_.writeFromNonRT( command_ptr );
// update failed, command size does not match number of joints
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::ERROR );
// check joint commands are still the default ones
ASSERT_EQ( joint_1_cmd_.get_value( ), 1.1 );
ASSERT_EQ( joint_2_cmd_.get_value( ), 2.1 );
ASSERT_EQ( joint_3_cmd_.get_value( ), 3.1 );
}
164 TEST_F( JointGroupEffortControllerTest, NoCommandCheckTest )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", joint_names_} );
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
// update successful, no command received yet
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
// check joint commands are still the default ones
ASSERT_EQ( joint_1_cmd_.get_value( ), 1.1 );
ASSERT_EQ( joint_2_cmd_.get_value( ), 2.1 );
ASSERT_EQ( joint_3_cmd_.get_value( ), 3.1 );
}
181 TEST_F( JointGroupEffortControllerTest, CommandCallbackTest )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", joint_names_} );
// default values
ASSERT_EQ( joint_1_cmd_.get_value( ), 1.1 );
ASSERT_EQ( joint_2_cmd_.get_value( ), 2.1 );
ASSERT_EQ( joint_3_cmd_.get_value( ), 3.1 );
auto node_state = controller_->get_node( )->configure( );
ASSERT_EQ( node_state.id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
node_state = controller_->get_node( )->activate( );
ASSERT_EQ( node_state.id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
// send a new command
rclcpp::Node test_node( "test_node" );
auto command_pub = test_node.create_publisher<std_msgs::msg::Float64MultiArray>(
std::string( controller_->get_node( )->get_name( ) ) + "/commands", rclcpp::SystemDefaultsQoS( ) );
std_msgs::msg::Float64MultiArray command_msg;
command_msg.data = {10.0, 20.0, 30.0};
command_pub->publish( command_msg );
// wait for command message to be passed
ASSERT_EQ( wait_for( controller_->joints_command_subscriber_ ), rclcpp::WaitResultKind::Ready );
// process callbacks
rclcpp::spin_some( controller_->get_node( )->get_node_base_interface( ) );
// update successful
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
// check command in handle was set
ASSERT_EQ( joint_1_cmd_.get_value( ), 10.0 );
ASSERT_EQ( joint_2_cmd_.get_value( ), 20.0 );
ASSERT_EQ( joint_3_cmd_.get_value( ), 30.0 );
}
222 TEST_F( JointGroupEffortControllerTest, StopJointsOnDeactivateTest )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", joint_names_} );
// configure successful
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
// check joint commands are still the default ones
ASSERT_EQ( joint_1_cmd_.get_value( ), 1.1 );
ASSERT_EQ( joint_2_cmd_.get_value( ), 2.1 );
ASSERT_EQ( joint_3_cmd_.get_value( ), 3.1 );
// stop the controller
ASSERT_EQ( controller_->on_deactivate( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
// check joint commands are now zero
ASSERT_EQ( joint_1_cmd_.get_value( ), 0.0 );
ASSERT_EQ( joint_2_cmd_.get_value( ), 0.0 );
ASSERT_EQ( joint_3_cmd_.get_value( ), 0.0 );
}
// Copyright 2020 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_
#define TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_
#include <memory>
#include <string>
#include <vector>
#include "gmock/gmock.h"
#include "effort_controllers/joint_group_effort_controller.hpp"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
using hardware_interface::CommandInterface;
using hardware_interface::HW_IF_EFFORT;
// subclassing and friending so we can access member variables
31 class FriendJointGroupEffortController : public effort_controllers::JointGroupEffortController
{
33 FRIEND_TEST( JointGroupEffortControllerTest, CommandSuccessTest );
34 FRIEND_TEST( JointGroupEffortControllerTest, WrongCommandCheckTest );
35 FRIEND_TEST( JointGroupEffortControllerTest, CommandCallbackTest );
36 FRIEND_TEST( JointGroupEffortControllerTest, StopJointsOnDeactivateTest );
};
39 class JointGroupEffortControllerTest : public ::testing::Test
{
public:
42 static void SetUpTestCase( );
43 static void TearDownTestCase( );
45 void SetUp( );
46 void TearDown( );
48 void SetUpController( );
protected:
51 std::unique_ptr<FriendJointGroupEffortController> controller_;
// dummy joint state values used for tests
54 const std::vector<std::string> joint_names_ = {"joint1", "joint2", "joint3"};
55 std::vector<double> joint_commands_ = {1.1, 2.1, 3.1};
CommandInterface joint_1_cmd_{joint_names_[0], HW_IF_EFFORT, &joint_commands_[0]};
58 CommandInterface joint_2_cmd_{joint_names_[1], HW_IF_EFFORT, &joint_commands_[1]};
CommandInterface joint_3_cmd_{joint_names_[2], HW_IF_EFFORT, &joint_commands_[2]};
};
#endif // TEST_JOINT_GROUP_EFFORT_CONTROLLER_HPP_
1 // Copyright 2020 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gmock/gmock.h>
#include <memory>
#include "controller_manager/controller_manager.hpp"
#include "hardware_interface/resource_manager.hpp"
#include "rclcpp/executors/single_threaded_executor.hpp"
#include "ros2_control_test_assets/descriptions.hpp"
23 TEST( TestLoadJointGroupVelocityController, load_controller )
{
rclcpp::init( 0, nullptr );
std::shared_ptr<rclcpp::Executor> executor =
std::make_shared<rclcpp::executors::SingleThreadedExecutor>( );
controller_manager::ControllerManager cm(
std::make_unique<hardware_interface::ResourceManager>(
ros2_control_test_assets::minimal_robot_urdf ),
executor, "test_controller_manager" );
ASSERT_NO_THROW( cm.load_controller(
"test_joint_group_effort_controller", "effort_controllers/JointGroupEffortController" ) );
rclcpp::shutdown( );
}
1 // Copyright ( c ) 2021, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*
* Authors: Subhas Das, Denis Stogl
*/
#ifndef FORCE_TORQUE_SENSOR_BROADCASTER__FORCE_TORQUE_SENSOR_BROADCASTER_HPP_
#define FORCE_TORQUE_SENSOR_BROADCASTER__FORCE_TORQUE_SENSOR_BROADCASTER_HPP_
#include <memory>
#include <string>
#include <vector>
#include "controller_interface/controller_interface.hpp"
#include "force_torque_sensor_broadcaster/visibility_control.h"
#include "geometry_msgs/msg/wrench_stamped.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "realtime_tools/realtime_publisher.h"
#include "semantic_components/force_torque_sensor.hpp"
namespace force_torque_sensor_broadcaster
{
36 class ForceTorqueSensorBroadcaster : public controller_interface::ControllerInterface
{
public:
FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
40 ForceTorqueSensorBroadcaster( );
FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
controller_interface::InterfaceConfiguration command_interface_configuration( ) const override;
FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
controller_interface::InterfaceConfiguration state_interface_configuration( ) const override;
FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init( ) override;
FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
controller_interface::CallbackReturn on_configure(
const rclcpp_lifecycle::State & previous_state ) override;
FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
controller_interface::CallbackReturn on_activate(
const rclcpp_lifecycle::State & previous_state ) override;
FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
controller_interface::CallbackReturn on_deactivate(
const rclcpp_lifecycle::State & previous_state ) override;
FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
controller_interface::return_type update(
const rclcpp::Time & time, const rclcpp::Duration & period ) override;
protected:
std::string sensor_name_;
std::array<std::string, 6> interface_names_;
std::string frame_id_;
std::unique_ptr<semantic_components::ForceTorqueSensor> force_torque_sensor_;
using StatePublisher = realtime_tools::RealtimePublisher<geometry_msgs::msg::WrenchStamped>;
rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr sensor_state_publisher_;
std::unique_ptr<StatePublisher> realtime_publisher_;
};
} // namespace force_torque_sensor_broadcaster
#endif // FORCE_TORQUE_SENSOR_BROADCASTER__FORCE_TORQUE_SENSOR_BROADCASTER_HPP_
1 // Copyright ( c ) 2021, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*
* Author: Subhas Das, Denis Stogl
*/
#ifndef FORCE_TORQUE_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_
#define FORCE_TORQUE_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_
// This logic was borrowed ( then namespaced ) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define FORCE_TORQUE_SENSOR_BROADCASTER_EXPORT __attribute__( ( dllexport ) )
#define FORCE_TORQUE_SENSOR_BROADCASTER_IMPORT __attribute__( ( dllimport ) )
#else
#define FORCE_TORQUE_SENSOR_BROADCASTER_EXPORT __declspec( dllexport )
#define FORCE_TORQUE_SENSOR_BROADCASTER_IMPORT __declspec( dllimport )
#endif
#ifdef FORCE_TORQUE_SENSOR_BROADCASTER_BUILDING_DLL
#define FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC FORCE_TORQUE_SENSOR_BROADCASTER_EXPORT
#else
#define FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC FORCE_TORQUE_SENSOR_BROADCASTER_IMPORT
#endif
#define FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC_TYPE FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
#define FORCE_TORQUE_SENSOR_BROADCASTER_LOCAL
#else
#define FORCE_TORQUE_SENSOR_BROADCASTER_EXPORT __attribute__( ( visibility( "default" ) ) )
#define FORCE_TORQUE_SENSOR_BROADCASTER_IMPORT
#if __GNUC__ >= 4
#define FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC __attribute__( ( visibility( "default" ) ) )
#define FORCE_TORQUE_SENSOR_BROADCASTER_LOCAL __attribute__( ( visibility( "hidden" ) ) )
#else
#define FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC
#define FORCE_TORQUE_SENSOR_BROADCASTER_LOCAL
#endif
#define FORCE_TORQUE_SENSOR_BROADCASTER_PUBLIC_TYPE
#endif
#endif // FORCE_TORQUE_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_
1 // Copyright ( c ) 2021, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*
* Authors: Subhas Das, Denis Stogl
*/
#include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp"
#include <memory>
#include <string>
namespace force_torque_sensor_broadcaster
{
26 ForceTorqueSensorBroadcaster::ForceTorqueSensorBroadcaster( )
: controller_interface::ControllerInterface( )
{
}
31 controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_init( )
{
try
{
auto_declare<std::string>( "sensor_name", "" );
auto_declare<std::string>( "interface_names.force.x", "" );
auto_declare<std::string>( "interface_names.force.y", "" );
auto_declare<std::string>( "interface_names.force.z", "" );
auto_declare<std::string>( "interface_names.torque.x", "" );
auto_declare<std::string>( "interface_names.torque.y", "" );
auto_declare<std::string>( "interface_names.torque.z", "" );
auto_declare<std::string>( "frame_id", "" );
}
catch ( const std::exception & e )
{
fprintf( stderr, "Exception thrown during init stage with message: %s \n", e.what( ) );
return controller_interface::CallbackReturn::ERROR;
}
return controller_interface::CallbackReturn::SUCCESS;
}
53 controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure(
54 const rclcpp_lifecycle::State & /*previous_state*/ )
{
sensor_name_ = get_node( )->get_parameter( "sensor_name" ).as_string( );
interface_names_[0] = get_node( )->get_parameter( "interface_names.force.x" ).as_string( );
interface_names_[1] = get_node( )->get_parameter( "interface_names.force.y" ).as_string( );
interface_names_[2] = get_node( )->get_parameter( "interface_names.force.z" ).as_string( );
interface_names_[3] = get_node( )->get_parameter( "interface_names.torque.x" ).as_string( );
interface_names_[4] = get_node( )->get_parameter( "interface_names.torque.y" ).as_string( );
interface_names_[5] = get_node( )->get_parameter( "interface_names.torque.z" ).as_string( );
const bool no_interface_names_defined =
std::count( interface_names_.begin( ), interface_names_.end( ), "" ) == 6;
if ( sensor_name_.empty( ) && no_interface_names_defined )
{
RCLCPP_ERROR(
get_node( )->get_logger( ),
"'sensor_name' or at least one "
"'interface_names.[force|torque].[x|y|z]' parameter has to be specified." );
return controller_interface::CallbackReturn::ERROR;
}
if ( !sensor_name_.empty( ) && !no_interface_names_defined )
{
RCLCPP_ERROR(
get_node( )->get_logger( ),
"both 'sensor_name' and "
"'interface_names.[force|torque].[x|y|z]' parameters can not be specified together." );
return controller_interface::CallbackReturn::ERROR;
}
frame_id_ = get_node( )->get_parameter( "frame_id" ).as_string( );
if ( frame_id_.empty( ) )
{
RCLCPP_ERROR( get_node( )->get_logger( ), "'frame_id' parameter has to be provided." );
return controller_interface::CallbackReturn::ERROR;
}
if ( !sensor_name_.empty( ) )
{
force_torque_sensor_ = std::make_unique<semantic_components::ForceTorqueSensor>(
semantic_components::ForceTorqueSensor( sensor_name_ ) );
}
else
{
force_torque_sensor_ = std::make_unique<semantic_components::ForceTorqueSensor>(
semantic_components::ForceTorqueSensor(
interface_names_[0], interface_names_[1], interface_names_[2], interface_names_[3],
interface_names_[4], interface_names_[5] ) );
}
try
{
// register ft sensor data publisher
sensor_state_publisher_ = get_node( )->create_publisher<geometry_msgs::msg::WrenchStamped>(
"~/wrench", rclcpp::SystemDefaultsQoS( ) );
realtime_publisher_ = std::make_unique<StatePublisher>( sensor_state_publisher_ );
}
catch ( const std::exception & e )
{
fprintf(
stderr, "Exception thrown during publisher creation at configure stage with message : %s \n",
e.what( ) );
return controller_interface::CallbackReturn::ERROR;
}
realtime_publisher_->lock( );
realtime_publisher_->msg_.header.frame_id = frame_id_;
realtime_publisher_->unlock( );
RCLCPP_DEBUG( get_node( )->get_logger( ), "configure successful" );
return controller_interface::CallbackReturn::SUCCESS;
}
controller_interface::InterfaceConfiguration
129 ForceTorqueSensorBroadcaster::command_interface_configuration( ) const
{
controller_interface::InterfaceConfiguration command_interfaces_config;
command_interfaces_config.type = controller_interface::interface_configuration_type::NONE;
return command_interfaces_config;
}
controller_interface::InterfaceConfiguration
137 ForceTorqueSensorBroadcaster::state_interface_configuration( ) const
{
controller_interface::InterfaceConfiguration state_interfaces_config;
state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
state_interfaces_config.names = force_torque_sensor_->get_state_interface_names( );
return state_interfaces_config;
}
145 controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_activate(
146 const rclcpp_lifecycle::State & /*previous_state*/ )
{
force_torque_sensor_->assign_loaned_state_interfaces( state_interfaces_ );
return controller_interface::CallbackReturn::SUCCESS;
}
152 controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_deactivate(
153 const rclcpp_lifecycle::State & /*previous_state*/ )
{
force_torque_sensor_->release_interfaces( );
return controller_interface::CallbackReturn::SUCCESS;
}
159 controller_interface::return_type ForceTorqueSensorBroadcaster::update(
160 const rclcpp::Time & time, const rclcpp::Duration & /*period*/ )
{
if ( realtime_publisher_ && realtime_publisher_->trylock( ) )
{
realtime_publisher_->msg_.header.stamp = time;
force_torque_sensor_->get_values_as_message( realtime_publisher_->msg_.wrench );
realtime_publisher_->unlockAndPublish( );
}
return controller_interface::return_type::OK;
}
} // namespace force_torque_sensor_broadcaster
#include "pluginlib/class_list_macros.hpp"
176 PLUGINLIB_EXPORT_CLASS(
force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster,
controller_interface::ControllerInterface )
// Copyright ( c ) 2021, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*
* Authors: Subhas Das, Denis Stogl
*/
#include "test_force_torque_sensor_broadcaster.hpp"
#include <memory>
#include <utility>
#include <vector>
#include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp"
#include "geometry_msgs/msg/wrench_stamped.hpp"
#include "hardware_interface/loaned_state_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/utilities.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
using hardware_interface::LoanedStateInterface;
namespace
{
38 constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS;
constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR;
} // namespace
void ForceTorqueSensorBroadcasterTest::SetUpTestCase( ) { rclcpp::init( 0, nullptr ); }
void ForceTorqueSensorBroadcasterTest::TearDownTestCase( ) { rclcpp::shutdown( ); }
void ForceTorqueSensorBroadcasterTest::SetUp( )
{
// initialize controller
fts_broadcaster_ = std::make_unique<FriendForceTorqueSensorBroadcaster>( );
}
void ForceTorqueSensorBroadcasterTest::TearDown( ) { fts_broadcaster_.reset( nullptr ); }
void ForceTorqueSensorBroadcasterTest::SetUpFTSBroadcaster( )
{
const auto result = fts_broadcaster_->init( "test_force_torque_sensor_broadcaster" );
ASSERT_EQ( result, controller_interface::return_type::OK );
std::vector<LoanedStateInterface> state_ifs;
state_ifs.emplace_back( fts_force_x_ );
state_ifs.emplace_back( fts_force_y_ );
state_ifs.emplace_back( fts_force_z_ );
state_ifs.emplace_back( fts_torque_x_ );
state_ifs.emplace_back( fts_torque_y_ );
state_ifs.emplace_back( fts_torque_z_ );
fts_broadcaster_->assign_interfaces( {}, std::move( state_ifs ) );
}
void ForceTorqueSensorBroadcasterTest::subscribe_and_get_message(
geometry_msgs::msg::WrenchStamped & wrench_msg )
{
// create a new subscriber
rclcpp::Node test_subscription_node( "test_subscription_node" );
auto subs_callback = [&]( const geometry_msgs::msg::WrenchStamped::SharedPtr ) {};
auto subscription = test_subscription_node.create_subscription<geometry_msgs::msg::WrenchStamped>(
"/test_force_torque_sensor_broadcaster/wrench", 10, subs_callback );
// call update to publish the test value
// since update doesn't guarantee a published message, republish until received
int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop
rclcpp::WaitSet wait_set; // block used to wait on message
wait_set.add_subscription( subscription );
while ( max_sub_check_loop_count-- )
{
fts_broadcaster_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
// check if message has been received
if ( wait_set.wait( std::chrono::milliseconds( 2 ) ).kind( ) == rclcpp::WaitResultKind::Ready )
{
break;
}
}
ASSERT_GE( max_sub_check_loop_count, 0 ) << "Test was unable to publish a message through "
"controller/broadcaster update loop";
// take message from subscription
rclcpp::MessageInfo msg_info;
ASSERT_TRUE( subscription->take( wrench_msg, msg_info ) );
}
TEST_F( ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_NotSet )
{
SetUpFTSBroadcaster( );
// configure failed
ASSERT_EQ( fts_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_ERROR );
}
TEST_F( ForceTorqueSensorBroadcasterTest, SensorName_FrameId_NotSet )
{
SetUpFTSBroadcaster( );
// set the 'interface_names'
fts_broadcaster_->get_node( )->set_parameter( {"interface_names.force.x", "fts_sensor/force.x"} );
fts_broadcaster_->get_node( )->set_parameter( {"interface_names.torque.z", "fts_sensor/torque.z"} );
// configure failed, 'frame_id' parameter not set
ASSERT_EQ( fts_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_ERROR );
}
TEST_F( ForceTorqueSensorBroadcasterTest, InterfaceNames_FrameId_NotSet )
{
SetUpFTSBroadcaster( );
// set the 'sensor_name'
fts_broadcaster_->get_node( )->set_parameter( {"sensor_name", sensor_name_} );
// configure failed, 'frame_id' parameter not set
ASSERT_EQ( fts_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_ERROR );
}
TEST_F( ForceTorqueSensorBroadcasterTest, SensorName_InterfaceNames_Set )
{
SetUpFTSBroadcaster( );
// set the 'sensor_name'
fts_broadcaster_->get_node( )->set_parameter( {"sensor_name", sensor_name_} );
// set the 'interface_names'
fts_broadcaster_->get_node( )->set_parameter( {"interface_names.force.x", "fts_sensor/force.x"} );
fts_broadcaster_->get_node( )->set_parameter( {"interface_names.torque.z", "fts_sensor/torque.z"} );
// configure failed, both 'sensor_name' and 'interface_names' supplied
ASSERT_EQ( fts_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_ERROR );
}
TEST_F( ForceTorqueSensorBroadcasterTest, SensorName_IsEmpty_InterfaceNames_NotSet )
{
SetUpFTSBroadcaster( );
// set the 'sensor_name' empty
fts_broadcaster_->get_node( )->set_parameter( {"sensor_name", ""} );
// configure failed, 'sensor_name' parameter empty
ASSERT_EQ( fts_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_ERROR );
}
TEST_F( ForceTorqueSensorBroadcasterTest, InterfaceNames_IsEmpty_SensorName_NotSet )
{
SetUpFTSBroadcaster( );
// set the 'interface_names' empty
fts_broadcaster_->get_node( )->set_parameter( {"interface_names.force.x", ""} );
fts_broadcaster_->get_node( )->set_parameter( {"interface_names.torque.z", ""} );
// configure failed, 'interface_name' parameter empty
ASSERT_EQ( fts_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_ERROR );
}
TEST_F( ForceTorqueSensorBroadcasterTest, SensorName_Configure_Success )
{
SetUpFTSBroadcaster( );
// set the 'sensor_name'
fts_broadcaster_->get_node( )->set_parameter( {"sensor_name", sensor_name_} );
// set the 'frame_id'
fts_broadcaster_->get_node( )->set_parameter( {"frame_id", frame_id_} );
// configure passed
ASSERT_EQ( fts_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
}
TEST_F( ForceTorqueSensorBroadcasterTest, InterfaceNames_Configure_Success )
{
SetUpFTSBroadcaster( );
// set the 'interface_names'
fts_broadcaster_->get_node( )->set_parameter( {"interface_names.force.x", "fts_sensor/force.x"} );
fts_broadcaster_->get_node( )->set_parameter( {"interface_names.torque.z", "fts_sensor/torque.z"} );
// set the 'frame_id'
fts_broadcaster_->get_node( )->set_parameter( {"frame_id", frame_id_} );
// configure passed
ASSERT_EQ( fts_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
}
TEST_F( ForceTorqueSensorBroadcasterTest, SensorName_Activate_Success )
{
SetUpFTSBroadcaster( );
// set the params 'sensor_name' and 'frame_id'
fts_broadcaster_->get_node( )->set_parameter( {"sensor_name", sensor_name_} );
fts_broadcaster_->get_node( )->set_parameter( {"frame_id", frame_id_} );
// configure and activate success
ASSERT_EQ( fts_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
ASSERT_EQ( fts_broadcaster_->on_activate( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
}
TEST_F( ForceTorqueSensorBroadcasterTest, SensorName_Update_Success )
{
SetUpFTSBroadcaster( );
// set the params 'sensor_name' and 'frame_id'
fts_broadcaster_->get_node( )->set_parameter( {"sensor_name", sensor_name_} );
fts_broadcaster_->get_node( )->set_parameter( {"frame_id", frame_id_} );
ASSERT_EQ( fts_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
ASSERT_EQ( fts_broadcaster_->on_activate( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
ASSERT_EQ(
fts_broadcaster_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
}
TEST_F( ForceTorqueSensorBroadcasterTest, InterfaceNames_Success )
{
SetUpFTSBroadcaster( );
// set the params 'interface_names' and 'frame_id'
fts_broadcaster_->get_node( )->set_parameter( {"interface_names.force.x", "fts_sensor/force.x"} );
fts_broadcaster_->get_node( )->set_parameter( {"interface_names.torque.z", "fts_sensor/torque.z"} );
fts_broadcaster_->get_node( )->set_parameter( {"frame_id", frame_id_} );
ASSERT_EQ( fts_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
ASSERT_EQ( fts_broadcaster_->on_activate( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
ASSERT_EQ(
fts_broadcaster_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
}
TEST_F( ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success )
{
SetUpFTSBroadcaster( );
// set the params 'sensor_name' and 'frame_id'
fts_broadcaster_->get_node( )->set_parameter( {"sensor_name", sensor_name_} );
fts_broadcaster_->get_node( )->set_parameter( {"frame_id", frame_id_} );
ASSERT_EQ( fts_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
ASSERT_EQ( fts_broadcaster_->on_activate( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
geometry_msgs::msg::WrenchStamped wrench_msg;
subscribe_and_get_message( wrench_msg );
ASSERT_EQ( wrench_msg.header.frame_id, frame_id_ );
ASSERT_EQ( wrench_msg.wrench.force.x, sensor_values_[0] );
ASSERT_EQ( wrench_msg.wrench.force.y, sensor_values_[1] );
ASSERT_EQ( wrench_msg.wrench.force.z, sensor_values_[2] );
ASSERT_EQ( wrench_msg.wrench.torque.x, sensor_values_[3] );
ASSERT_EQ( wrench_msg.wrench.torque.y, sensor_values_[4] );
ASSERT_EQ( wrench_msg.wrench.torque.z, sensor_values_[5] );
}
TEST_F( ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success )
{
SetUpFTSBroadcaster( );
// set the params 'interface_names' and 'frame_id'
fts_broadcaster_->get_node( )->set_parameter( {"interface_names.force.x", "fts_sensor/force.x"} );
fts_broadcaster_->get_node( )->set_parameter( {"interface_names.torque.z", "fts_sensor/torque.z"} );
fts_broadcaster_->get_node( )->set_parameter( {"frame_id", frame_id_} );
ASSERT_EQ( fts_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
ASSERT_EQ( fts_broadcaster_->on_activate( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
geometry_msgs::msg::WrenchStamped wrench_msg;
subscribe_and_get_message( wrench_msg );
ASSERT_EQ( wrench_msg.header.frame_id, frame_id_ );
ASSERT_EQ( wrench_msg.wrench.force.x, sensor_values_[0] );
ASSERT_TRUE( std::isnan( wrench_msg.wrench.force.y ) );
ASSERT_TRUE( std::isnan( wrench_msg.wrench.force.z ) );
ASSERT_TRUE( std::isnan( wrench_msg.wrench.torque.x ) );
ASSERT_TRUE( std::isnan( wrench_msg.wrench.torque.y ) );
ASSERT_EQ( wrench_msg.wrench.torque.z, sensor_values_[5] );
}
TEST_F( ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success )
{
SetUpFTSBroadcaster( );
// set all the params 'interface_names' and 'frame_id'
fts_broadcaster_->get_node( )->set_parameter( {"interface_names.force.x", "fts_sensor/force.x"} );
fts_broadcaster_->get_node( )->set_parameter( {"interface_names.force.y", "fts_sensor/force.y"} );
fts_broadcaster_->get_node( )->set_parameter( {"interface_names.force.z", "fts_sensor/force.z"} );
fts_broadcaster_->get_node( )->set_parameter( {"interface_names.torque.x", "fts_sensor/torque.x"} );
fts_broadcaster_->get_node( )->set_parameter( {"interface_names.torque.y", "fts_sensor/torque.y"} );
fts_broadcaster_->get_node( )->set_parameter( {"interface_names.torque.z", "fts_sensor/torque.z"} );
fts_broadcaster_->get_node( )->set_parameter( {"frame_id", frame_id_} );
ASSERT_EQ( fts_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
ASSERT_EQ( fts_broadcaster_->on_activate( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
geometry_msgs::msg::WrenchStamped wrench_msg;
subscribe_and_get_message( wrench_msg );
ASSERT_EQ( wrench_msg.header.frame_id, frame_id_ );
ASSERT_EQ( wrench_msg.wrench.force.x, sensor_values_[0] );
ASSERT_EQ( wrench_msg.wrench.force.y, sensor_values_[1] );
ASSERT_EQ( wrench_msg.wrench.force.z, sensor_values_[2] );
ASSERT_EQ( wrench_msg.wrench.torque.x, sensor_values_[3] );
ASSERT_EQ( wrench_msg.wrench.torque.y, sensor_values_[4] );
ASSERT_EQ( wrench_msg.wrench.torque.z, sensor_values_[5] );
}
// Copyright ( c ) 2021, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*
* Authors: Subhas Das, Denis Stogl
*/
#ifndef TEST_FORCE_TORQUE_SENSOR_BROADCASTER_HPP_
#define TEST_FORCE_TORQUE_SENSOR_BROADCASTER_HPP_
#include <memory>
#include <string>
#include "gmock/gmock.h"
#include "force_torque_sensor_broadcaster/force_torque_sensor_broadcaster.hpp"
// subclassing and friending so we can access member variables
30 class FriendForceTorqueSensorBroadcaster
31 : public force_torque_sensor_broadcaster::ForceTorqueSensorBroadcaster
{
33 FRIEND_TEST( ForceTorqueSensorBroadcasterTest, SensorNameParameterNotSet );
34 FRIEND_TEST( ForceTorqueSensorBroadcasterTest, InterfaceNamesParameterNotSet );
35 FRIEND_TEST( ForceTorqueSensorBroadcasterTest, FrameIdParameterNotSet );
36 FRIEND_TEST( ForceTorqueSensorBroadcasterTest, SensorNameParameterIsEmpty );
37 FRIEND_TEST( ForceTorqueSensorBroadcasterTest, InterfaceNameParameterIsEmpty );
39 FRIEND_TEST( ForceTorqueSensorBroadcasterTest, ActivateSuccess );
40 FRIEND_TEST( ForceTorqueSensorBroadcasterTest, UpdateTest );
41 FRIEND_TEST( ForceTorqueSensorBroadcasterTest, SensorStatePublishTest );
};
44 class ForceTorqueSensorBroadcasterTest : public ::testing::Test
{
public:
47 static void SetUpTestCase( );
48 static void TearDownTestCase( );
50 void SetUp( );
51 void TearDown( );
53 void SetUpFTSBroadcaster( );
protected:
56 const std::string sensor_name_ = "fts_sensor";
57 const std::string frame_id_ = "fts_sensor_frame";
58 std::array<double, 6> sensor_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6};
hardware_interface::StateInterface fts_force_x_{sensor_name_, "force.x", &sensor_values_[0]};
61 hardware_interface::StateInterface fts_force_y_{sensor_name_, "force.y", &sensor_values_[1]};
hardware_interface::StateInterface fts_force_z_{sensor_name_, "force.z", &sensor_values_[2]};
hardware_interface::StateInterface fts_torque_x_{sensor_name_, "torque.x", &sensor_values_[3]};
hardware_interface::StateInterface fts_torque_y_{sensor_name_, "torque.y", &sensor_values_[4]};
hardware_interface::StateInterface fts_torque_z_{sensor_name_, "torque.z", &sensor_values_[5]};
std::unique_ptr<FriendForceTorqueSensorBroadcaster> fts_broadcaster_;
void subscribe_and_get_message( geometry_msgs::msg::WrenchStamped & wrench_msg );
};
#endif // TEST_FORCE_TORQUE_SENSOR_BROADCASTER_HPP_
1 // Copyright ( c ) 2021, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*
* Author: Subhas Das, Denis Stogl
*/
#include <gmock/gmock.h>
#include <memory>
#include "controller_manager/controller_manager.hpp"
#include "hardware_interface/resource_manager.hpp"
#include "rclcpp/executor.hpp"
#include "rclcpp/executors/single_threaded_executor.hpp"
#include "rclcpp/utilities.hpp"
#include "ros2_control_test_assets/descriptions.hpp"
29 TEST( TestLoadForceTorqueSensorBroadcaster, load_controller )
{
rclcpp::init( 0, nullptr );
std::shared_ptr<rclcpp::Executor> executor =
std::make_shared<rclcpp::executors::SingleThreadedExecutor>( );
controller_manager::ControllerManager cm(
std::make_unique<hardware_interface::ResourceManager>(
ros2_control_test_assets::minimal_robot_urdf ),
executor, "test_controller_manager" );
ASSERT_NO_THROW( cm.load_controller(
"test_force_torque_sensor_broadcaster",
"force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster" ) );
rclcpp::shutdown( );
}
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_
#define FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_
#include <string>
#include <vector>
#include "forward_command_controller/forward_controllers_base.hpp"
#include "forward_command_controller/visibility_control.h"
namespace forward_command_controller
{
/**
* \brief Forward command controller for a set of joints.
*
* This class forwards the command signal down to a set of joints on the specified interface.
*
* \param joints Names of the joints to control.
* \param interface_name Name of the interface to command.
*
* Subscribes to:
* - \b commands ( std_msgs::msg::Float64MultiArray ) : The commands to apply.
*/
37 class ForwardCommandController : public ForwardControllersBase
{
public:
FORWARD_COMMAND_CONTROLLER_PUBLIC
41 ForwardCommandController( );
protected:
void declare_parameters( ) override;
controller_interface::CallbackReturn read_parameters( ) override;
std::vector<std::string> joint_names_;
std::string interface_name_;
};
} // namespace forward_command_controller
#endif // FORWARD_COMMAND_CONTROLLER__FORWARD_COMMAND_CONTROLLER_HPP_
1 // Copyright 2021 Stogl Robotics Consulting UG ( haftungsbescrhänkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef FORWARD_COMMAND_CONTROLLER__FORWARD_CONTROLLERS_BASE_HPP_
#define FORWARD_COMMAND_CONTROLLER__FORWARD_CONTROLLERS_BASE_HPP_
#include <memory>
#include <string>
#include <vector>
#include "controller_interface/controller_interface.hpp"
#include "forward_command_controller/visibility_control.h"
#include "rclcpp/subscription.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "realtime_tools/realtime_buffer.h"
#include "std_msgs/msg/float64_multi_array.hpp"
namespace forward_command_controller
{
using CmdType = std_msgs::msg::Float64MultiArray;
/**
* \brief Forward command controller for a set of joints and interfaces.
*
* This class forwards the command signal down to a set of joints or interfaces.
*
* Subscribes to:
* - \b commands ( std_msgs::msg::Float64MultiArray ) : The commands to apply.
*/
42 class ForwardControllersBase : public controller_interface::ControllerInterface
{
public:
FORWARD_COMMAND_CONTROLLER_PUBLIC
46 ForwardControllersBase( );
FORWARD_COMMAND_CONTROLLER_PUBLIC
~ForwardControllersBase( ) = default;
FORWARD_COMMAND_CONTROLLER_PUBLIC
controller_interface::InterfaceConfiguration command_interface_configuration( ) const override;
FORWARD_COMMAND_CONTROLLER_PUBLIC
controller_interface::InterfaceConfiguration state_interface_configuration( ) const override;
FORWARD_COMMAND_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_init( ) override;
FORWARD_COMMAND_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_configure(
const rclcpp_lifecycle::State & previous_state ) override;
FORWARD_COMMAND_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_activate(
const rclcpp_lifecycle::State & previous_state ) override;
FORWARD_COMMAND_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_deactivate(
const rclcpp_lifecycle::State & previous_state ) override;
FORWARD_COMMAND_CONTROLLER_PUBLIC
controller_interface::return_type update(
const rclcpp::Time & time, const rclcpp::Duration & period ) override;
protected:
/**
* Derived controllers have to declare parameters in this method.
* Error handling does not have to be done. It is done in `on_init`-method of this class.
*/
virtual void declare_parameters( ) = 0;
/**
* Derived controllers have to read parameters in this method and set `command_interface_types_`
* variable. The variable is then used to propagate the command interface configuration to
* controller manager. The method is called from `on_configure`-method of this class.
*
* It is expected that error handling of exceptions is done.
*
* \returns controller_interface::CallbackReturn::SUCCESS if parameters are successfully read and their values are
* allowed, controller_interface::CallbackReturn::ERROR otherwise.
*/
virtual controller_interface::CallbackReturn read_parameters( ) = 0;
std::vector<std::string> joint_names_;
std::string interface_name_;
std::vector<std::string> command_interface_types_;
realtime_tools::RealtimeBuffer<std::shared_ptr<CmdType>> rt_command_ptr_;
rclcpp::Subscription<CmdType>::SharedPtr joints_command_subscriber_;
};
} // namespace forward_command_controller
#endif // FORWARD_COMMAND_CONTROLLER__FORWARD_CONTROLLERS_BASE_HPP_
1 // Copyright 2021 Stogl Robotics Consulting UG ( haftungsbescrhänkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef FORWARD_COMMAND_CONTROLLER__MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_
#define FORWARD_COMMAND_CONTROLLER__MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_
#include <string>
#include <vector>
#include "forward_command_controller/forward_controllers_base.hpp"
#include "forward_command_controller/visibility_control.h"
namespace forward_command_controller
{
/**
* \brief Multi interface forward command controller for a set of interfaces.
*
* This class forwards the command signal down to a set of interfaces on the specified joint.
*
* \param joint Name of the joint to control.
* \param interface_names Names of the interfaces to command.
*
* Subscribes to:
* - \b commands ( std_msgs::msg::Float64MultiArray ) : The commands to apply.
*/
37 class MultiInterfaceForwardCommandController
38 : public forward_command_controller::ForwardControllersBase
{
public:
FORWARD_COMMAND_CONTROLLER_PUBLIC
42 MultiInterfaceForwardCommandController( );
protected:
void declare_parameters( ) override;
controller_interface::CallbackReturn read_parameters( ) override;
std::string joint_name_;
std::vector<std::string> interface_names_;
};
} // namespace forward_command_controller
#endif // FORWARD_COMMAND_CONTROLLER__MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/* This header must be included by all rclcpp headers which declare symbols
* which are defined in the rclcpp library. When not building the rclcpp
* library, i.e. when using the headers in other package's code, the contents
* of this header change the visibility of certain symbols which the rclcpp
* library cannot have, but the consuming code must have inorder to link.
*/
#ifndef FORWARD_COMMAND_CONTROLLER__VISIBILITY_CONTROL_H_
#define FORWARD_COMMAND_CONTROLLER__VISIBILITY_CONTROL_H_
// This logic was borrowed ( then namespaced ) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define FORWARD_COMMAND_CONTROLLER_EXPORT __attribute__( ( dllexport ) )
#define FORWARD_COMMAND_CONTROLLER_IMPORT __attribute__( ( dllimport ) )
#else
#define FORWARD_COMMAND_CONTROLLER_EXPORT __declspec( dllexport )
#define FORWARD_COMMAND_CONTROLLER_IMPORT __declspec( dllimport )
#endif
#ifdef FORWARD_COMMAND_CONTROLLER_BUILDING_DLL
#define FORWARD_COMMAND_CONTROLLER_PUBLIC FORWARD_COMMAND_CONTROLLER_EXPORT
#else
#define FORWARD_COMMAND_CONTROLLER_PUBLIC FORWARD_COMMAND_CONTROLLER_IMPORT
#endif
#define FORWARD_COMMAND_CONTROLLER_PUBLIC_TYPE FORWARD_COMMAND_CONTROLLER_PUBLIC
#define FORWARD_COMMAND_CONTROLLER_LOCAL
#else
#define FORWARD_COMMAND_CONTROLLER_EXPORT __attribute__( ( visibility( "default" ) ) )
#define FORWARD_COMMAND_CONTROLLER_IMPORT
#if __GNUC__ >= 4
#define FORWARD_COMMAND_CONTROLLER_PUBLIC __attribute__( ( visibility( "default" ) ) )
#define FORWARD_COMMAND_CONTROLLER_LOCAL __attribute__( ( visibility( "hidden" ) ) )
#else
#define FORWARD_COMMAND_CONTROLLER_PUBLIC
#define FORWARD_COMMAND_CONTROLLER_LOCAL
#endif
#define FORWARD_COMMAND_CONTROLLER_PUBLIC_TYPE
#endif
#endif // FORWARD_COMMAND_CONTROLLER__VISIBILITY_CONTROL_H_
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "forward_command_controller/forward_command_controller.hpp"
#include <algorithm>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "rclcpp/logging.hpp"
#include "rclcpp/qos.hpp"
namespace forward_command_controller
{
28 ForwardCommandController::ForwardCommandController( ) : ForwardControllersBase( ) {}
30 void ForwardCommandController::declare_parameters( )
{
auto_declare( "joints", std::vector<std::string>( ) );
auto_declare( "interface_name", std::string( ) );
}
36 controller_interface::CallbackReturn ForwardCommandController::read_parameters( )
{
joint_names_ = get_node( )->get_parameter( "joints" ).as_string_array( );
if ( joint_names_.empty( ) )
{
RCLCPP_ERROR( get_node( )->get_logger( ), "'joints' parameter was empty" );
return controller_interface::CallbackReturn::ERROR;
}
// Specialized, child controllers set interfaces before calling configure function.
if ( interface_name_.empty( ) )
{
interface_name_ = get_node( )->get_parameter( "interface_name" ).as_string( );
}
if ( interface_name_.empty( ) )
{
RCLCPP_ERROR( get_node( )->get_logger( ), "'interface_name' parameter was empty" );
return controller_interface::CallbackReturn::ERROR;
}
for ( const auto & joint : joint_names_ )
{
command_interface_types_.push_back( joint + "/" + interface_name_ );
}
return controller_interface::CallbackReturn::SUCCESS;
}
} // namespace forward_command_controller
#include "pluginlib/class_list_macros.hpp"
70 PLUGINLIB_EXPORT_CLASS(
forward_command_controller::ForwardCommandController, controller_interface::ControllerInterface )
1 // Copyright 2021 Stogl Robotics Consulting UG ( haftungsbescrhänkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "forward_command_controller/forward_controllers_base.hpp"
#include <algorithm>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "controller_interface/helpers.hpp"
#include "hardware_interface/loaned_command_interface.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/qos.hpp"
namespace forward_command_controller
{
30 ForwardControllersBase::ForwardControllersBase( )
: controller_interface::ControllerInterface( ),
rt_command_ptr_( nullptr ),
joints_command_subscriber_( nullptr )
{
}
37 controller_interface::CallbackReturn ForwardControllersBase::on_init( )
{
try
{
declare_parameters( );
}
catch ( const std::exception & e )
{
fprintf( stderr, "Exception thrown during init stage with message: %s \n", e.what( ) );
return controller_interface::CallbackReturn::ERROR;
}
return controller_interface::CallbackReturn::SUCCESS;
}
52 controller_interface::CallbackReturn ForwardControllersBase::on_configure(
53 const rclcpp_lifecycle::State & /*previous_state*/ )
{
auto ret = this->read_parameters( );
if ( ret != controller_interface::CallbackReturn::SUCCESS )
{
return ret;
}
joints_command_subscriber_ = get_node( )->create_subscription<CmdType>(
"~/commands", rclcpp::SystemDefaultsQoS( ),
[this]( const CmdType::SharedPtr msg ) { rt_command_ptr_.writeFromNonRT( msg ); } );
RCLCPP_INFO( get_node( )->get_logger( ), "configure successful" );
return controller_interface::CallbackReturn::SUCCESS;
}
controller_interface::InterfaceConfiguration
70 ForwardControllersBase::command_interface_configuration( ) const
{
controller_interface::InterfaceConfiguration command_interfaces_config;
command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
command_interfaces_config.names = command_interface_types_;
return command_interfaces_config;
}
79 controller_interface::InterfaceConfiguration ForwardControllersBase::state_interface_configuration( )
const
{
return controller_interface::InterfaceConfiguration{
controller_interface::interface_configuration_type::NONE};
}
86 controller_interface::CallbackReturn ForwardControllersBase::on_activate(
87 const rclcpp_lifecycle::State & /*previous_state*/ )
{
// check if we have all resources defined in the "points" parameter
// also verify that we *only* have the resources defined in the "points" parameter
// ATTENTION( destogl ): Shouldn't we use ordered interface all the time?
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
ordered_interfaces;
if (
!controller_interface::get_ordered_interfaces(
command_interfaces_, command_interface_types_, std::string( "" ), ordered_interfaces ) ||
command_interface_types_.size( ) != ordered_interfaces.size( ) )
{
RCLCPP_ERROR(
get_node( )->get_logger( ), "Expected %zu command interfaces, got %zu",
command_interface_types_.size( ), ordered_interfaces.size( ) );
return controller_interface::CallbackReturn::ERROR;
}
// reset command buffer if a command came through callback when controller was inactive
rt_command_ptr_ = realtime_tools::RealtimeBuffer<std::shared_ptr<CmdType>>( nullptr );
RCLCPP_INFO( get_node( )->get_logger( ), "activate successful" );
return controller_interface::CallbackReturn::SUCCESS;
}
112 controller_interface::CallbackReturn ForwardControllersBase::on_deactivate(
113 const rclcpp_lifecycle::State & /*previous_state*/ )
{
// reset command buffer
rt_command_ptr_ = realtime_tools::RealtimeBuffer<std::shared_ptr<CmdType>>( nullptr );
return controller_interface::CallbackReturn::SUCCESS;
}
120 controller_interface::return_type ForwardControllersBase::update(
121 const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ )
{
auto joint_commands = rt_command_ptr_.readFromRT( );
// no command received yet
if ( !joint_commands || !( *joint_commands ) )
{
return controller_interface::return_type::OK;
}
if ( ( *joint_commands )->data.size( ) != command_interfaces_.size( ) )
{
RCLCPP_ERROR_THROTTLE(
get_node( )->get_logger( ), *( get_node( )->get_clock( ) ), 1000,
"command size ( %zu ) does not match number of interfaces ( %zu )",
( *joint_commands )->data.size( ), command_interfaces_.size( ) );
return controller_interface::return_type::ERROR;
}
for ( auto index = 0ul; index < command_interfaces_.size( ); ++index )
{
command_interfaces_[index].set_value( ( *joint_commands )->data[index] );
}
return controller_interface::return_type::OK;
}
} // namespace forward_command_controller
1 // Copyright 2021 Stogl Robotics Consulting UG ( haftungsbescrhänkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "forward_command_controller/multi_interface_forward_command_controller.hpp"
#include <string>
#include <vector>
namespace forward_command_controller
{
22 MultiInterfaceForwardCommandController::MultiInterfaceForwardCommandController( )
: ForwardControllersBase( )
{
}
27 void MultiInterfaceForwardCommandController::declare_parameters( )
{
auto_declare( "joint", joint_name_ );
auto_declare( "interface_names", interface_names_ );
}
33 controller_interface::CallbackReturn MultiInterfaceForwardCommandController::read_parameters( )
{
joint_name_ = get_node( )->get_parameter( "joint" ).as_string( );
interface_names_ = get_node( )->get_parameter( "interface_names" ).as_string_array( );
if ( joint_name_.empty( ) )
{
RCLCPP_ERROR( get_node( )->get_logger( ), "'joint' parameter is empty" );
return controller_interface::CallbackReturn::ERROR;
}
if ( interface_names_.empty( ) )
{
RCLCPP_ERROR( get_node( )->get_logger( ), "'interfaces' parameter is empty" );
return controller_interface::CallbackReturn::ERROR;
}
for ( const auto & interface : interface_names_ )
{
command_interface_types_.push_back( joint_name_ + "/" + interface );
}
return controller_interface::CallbackReturn::SUCCESS;
}
} // namespace forward_command_controller
#include "pluginlib/class_list_macros.hpp"
62 PLUGINLIB_EXPORT_CLASS(
forward_command_controller::MultiInterfaceForwardCommandController,
controller_interface::ControllerInterface )
1 // Copyright 2020 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <functional>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "gmock/gmock.h"
#include "test_forward_command_controller.hpp"
#include "forward_command_controller/forward_command_controller.hpp"
#include "hardware_interface/loaned_command_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/qos.hpp"
#include "rclcpp/subscription.hpp"
#include "rclcpp/utilities.hpp"
#include "rclcpp/wait_set.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
using hardware_interface::LoanedCommandInterface;
namespace
{
40 rclcpp::WaitResultKind wait_for( rclcpp::SubscriptionBase::SharedPtr subscription )
{
rclcpp::WaitSet wait_set;
wait_set.add_subscription( subscription );
const auto timeout = std::chrono::seconds( 10 );
return wait_set.wait( timeout ).kind( );
}
} // namespace
49 void ForwardCommandControllerTest::SetUpTestCase( ) { rclcpp::init( 0, nullptr ); }
51 void ForwardCommandControllerTest::TearDownTestCase( ) { rclcpp::shutdown( ); }
53 void ForwardCommandControllerTest::SetUp( )
{
// initialize controller
controller_ = std::make_unique<FriendForwardCommandController>( );
}
59 void ForwardCommandControllerTest::TearDown( ) { controller_.reset( nullptr ); }
61 void ForwardCommandControllerTest::SetUpController( )
{
const auto result = controller_->init( "forward_command_controller" );
ASSERT_EQ( result, controller_interface::return_type::OK );
std::vector<LoanedCommandInterface> command_ifs;
command_ifs.emplace_back( joint_1_pos_cmd_ );
command_ifs.emplace_back( joint_2_pos_cmd_ );
command_ifs.emplace_back( joint_3_pos_cmd_ );
controller_->assign_interfaces( std::move( command_ifs ), {} );
}
73 TEST_F( ForwardCommandControllerTest, JointsParameterNotSet )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"interface_name", ""} );
// configure failed, 'joints' parameter not set
ASSERT_EQ(
controller_->on_configure( rclcpp_lifecycle::State( ) ),
controller_interface::CallbackReturn::ERROR );
}
84 TEST_F( ForwardCommandControllerTest, InterfaceParameterNotSet )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", std::vector<std::string>( )} );
// configure failed, 'interface_name' parameter not set
ASSERT_EQ(
controller_->on_configure( rclcpp_lifecycle::State( ) ),
controller_interface::CallbackReturn::ERROR );
}
95 TEST_F( ForwardCommandControllerTest, JointsParameterIsEmpty )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", std::vector<std::string>( )} );
controller_->get_node( )->set_parameter( {"interface_name", ""} );
// configure failed, 'joints' is empty
ASSERT_EQ(
controller_->on_configure( rclcpp_lifecycle::State( ) ),
controller_interface::CallbackReturn::ERROR );
}
108 TEST_F( ForwardCommandControllerTest, InterfaceParameterEmpty )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", std::vector<std::string>{"joint1", "joint2"}} );
controller_->get_node( )->set_parameter( {"interface_name", ""} );
// configure failed, 'interface_name' is empty
ASSERT_EQ(
controller_->on_configure( rclcpp_lifecycle::State( ) ),
controller_interface::CallbackReturn::ERROR );
}
120 TEST_F( ForwardCommandControllerTest, ConfigureParamsSuccess )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", std::vector<std::string>{"joint1", "joint2"}} );
controller_->get_node( )->set_parameter( {"interface_name", "position"} );
// configure successful
ASSERT_EQ(
controller_->on_configure( rclcpp_lifecycle::State( ) ),
controller_interface::CallbackReturn::SUCCESS );
}
133 TEST_F( ForwardCommandControllerTest, ActivateWithWrongJointsNamesFails )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", std::vector<std::string>{"joint1", "joint4"}} );
controller_->get_node( )->set_parameter( {"interface_name", "position"} );
// activate failed, 'joint4' is not a valid joint name for the hardware
ASSERT_EQ(
controller_->on_configure( rclcpp_lifecycle::State( ) ),
controller_interface::CallbackReturn::SUCCESS );
ASSERT_EQ(
controller_->on_activate( rclcpp_lifecycle::State( ) ),
controller_interface::CallbackReturn::ERROR );
}
149 TEST_F( ForwardCommandControllerTest, ActivateWithWrongInterfaceNameFails )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", joint_names_} );
controller_->get_node( )->set_parameter( {"interface_name", "acceleration"} );
// activate failed, 'acceleration' is not a registered interface for `joint1`
ASSERT_EQ(
controller_->on_configure( rclcpp_lifecycle::State( ) ),
controller_interface::CallbackReturn::SUCCESS );
ASSERT_EQ(
controller_->on_activate( rclcpp_lifecycle::State( ) ),
controller_interface::CallbackReturn::ERROR );
}
165 TEST_F( ForwardCommandControllerTest, ActivateSuccess )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", joint_names_} );
controller_->get_node( )->set_parameter( {"interface_name", "position"} );
// activate successful
ASSERT_EQ(
controller_->on_configure( rclcpp_lifecycle::State( ) ),
controller_interface::CallbackReturn::SUCCESS );
ASSERT_EQ(
controller_->on_activate( rclcpp_lifecycle::State( ) ),
controller_interface::CallbackReturn::SUCCESS );
}
181 TEST_F( ForwardCommandControllerTest, CommandSuccessTest )
{
SetUpController( );
// configure controller
controller_->get_node( )->set_parameter( {"joints", joint_names_} );
controller_->get_node( )->set_parameter( {"interface_name", "position"} );
ASSERT_EQ(
controller_->on_configure( rclcpp_lifecycle::State( ) ),
controller_interface::CallbackReturn::SUCCESS );
// update successful though no command has been send yet
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
// check joint commands are still the default ones
ASSERT_EQ( joint_1_pos_cmd_.get_value( ), 1.1 );
ASSERT_EQ( joint_2_pos_cmd_.get_value( ), 2.1 );
ASSERT_EQ( joint_3_pos_cmd_.get_value( ), 3.1 );
// send command
auto command_ptr = std::make_shared<forward_command_controller::CmdType>( );
command_ptr->data = {10.0, 20.0, 30.0};
controller_->rt_command_ptr_.writeFromNonRT( command_ptr );
// update successful, command received
ASSERT_EQ(
controller_->update( rclcpp::Time( 0.1 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
// check joint commands have been modified
ASSERT_EQ( joint_1_pos_cmd_.get_value( ), 10.0 );
ASSERT_EQ( joint_2_pos_cmd_.get_value( ), 20.0 );
ASSERT_EQ( joint_3_pos_cmd_.get_value( ), 30.0 );
}
218 TEST_F( ForwardCommandControllerTest, WrongCommandCheckTest )
{
SetUpController( );
// configure controller
controller_->get_node( )->set_parameter( {"joints", joint_names_} );
controller_->get_node( )->set_parameter( {"interface_name", "position"} );
ASSERT_EQ(
controller_->on_configure( rclcpp_lifecycle::State( ) ),
controller_interface::CallbackReturn::SUCCESS );
// send command with wrong number of joints
auto command_ptr = std::make_shared<forward_command_controller::CmdType>( );
command_ptr->data = {10.0, 20.0};
controller_->rt_command_ptr_.writeFromNonRT( command_ptr );
// update failed, command size does not match number of joints
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::ERROR );
// check joint commands are still the default ones
ASSERT_EQ( joint_1_pos_cmd_.get_value( ), 1.1 );
ASSERT_EQ( joint_2_pos_cmd_.get_value( ), 2.1 );
ASSERT_EQ( joint_3_pos_cmd_.get_value( ), 3.1 );
}
246 TEST_F( ForwardCommandControllerTest, NoCommandCheckTest )
{
SetUpController( );
// configure controller
controller_->get_node( )->set_parameter( {"joints", joint_names_} );
controller_->get_node( )->set_parameter( {"interface_name", "position"} );
ASSERT_EQ(
controller_->on_configure( rclcpp_lifecycle::State( ) ),
controller_interface::CallbackReturn::SUCCESS );
// update successful, no command received yet
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
// check joint commands are still the default ones
ASSERT_EQ( joint_1_pos_cmd_.get_value( ), 1.1 );
ASSERT_EQ( joint_2_pos_cmd_.get_value( ), 2.1 );
ASSERT_EQ( joint_3_pos_cmd_.get_value( ), 3.1 );
}
268 TEST_F( ForwardCommandControllerTest, CommandCallbackTest )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", joint_names_} );
controller_->get_node( )->set_parameter( {"interface_name", "position"} );
// default values
ASSERT_EQ( joint_1_pos_cmd_.get_value( ), 1.1 );
ASSERT_EQ( joint_2_pos_cmd_.get_value( ), 2.1 );
ASSERT_EQ( joint_3_pos_cmd_.get_value( ), 3.1 );
auto node_state = controller_->get_node( )->configure( );
ASSERT_EQ( node_state.id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
node_state = controller_->get_node( )->activate( );
ASSERT_EQ( node_state.id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
// send a new command
rclcpp::Node test_node( "test_node" );
auto command_pub = test_node.create_publisher<std_msgs::msg::Float64MultiArray>(
std::string( controller_->get_node( )->get_name( ) ) + "/commands", rclcpp::SystemDefaultsQoS( ) );
std_msgs::msg::Float64MultiArray command_msg;
command_msg.data = {10.0, 20.0, 30.0};
command_pub->publish( command_msg );
// wait for command message to be passed
ASSERT_EQ( wait_for( controller_->joints_command_subscriber_ ), rclcpp::WaitResultKind::Ready );
// process callbacks
rclcpp::spin_some( controller_->get_node( )->get_node_base_interface( ) );
// update successful
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
// check command in handle was set
ASSERT_EQ( joint_1_pos_cmd_.get_value( ), 10.0 );
ASSERT_EQ( joint_2_pos_cmd_.get_value( ), 20.0 );
ASSERT_EQ( joint_3_pos_cmd_.get_value( ), 30.0 );
}
311 TEST_F( ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", joint_names_} );
controller_->get_node( )->set_parameter( {"interface_name", "position"} );
// default values
ASSERT_EQ( joint_1_pos_cmd_.get_value( ), 1.1 );
ASSERT_EQ( joint_2_pos_cmd_.get_value( ), 2.1 );
ASSERT_EQ( joint_3_pos_cmd_.get_value( ), 3.1 );
auto node_state = controller_->configure( );
ASSERT_EQ( node_state.id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
node_state = controller_->get_node( )->activate( );
ASSERT_EQ( node_state.id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
auto command_msg = std::make_shared<std_msgs::msg::Float64MultiArray>( );
command_msg->data = {10.0, 20.0, 30.0};
controller_->rt_command_ptr_.writeFromNonRT( command_msg );
// update successful
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
// check command in handle was set
ASSERT_EQ( joint_1_pos_cmd_.get_value( ), 10 );
ASSERT_EQ( joint_2_pos_cmd_.get_value( ), 20 );
ASSERT_EQ( joint_3_pos_cmd_.get_value( ), 30 );
node_state = controller_->get_node( )->deactivate( );
ASSERT_EQ( node_state.id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
// command ptr should be reset ( nullptr ) after deactivation - same check as in `update`
ASSERT_FALSE(
controller_->rt_command_ptr_.readFromNonRT( ) &&
*( controller_->rt_command_ptr_.readFromNonRT( ) ) );
ASSERT_FALSE(
controller_->rt_command_ptr_.readFromRT( ) && *( controller_->rt_command_ptr_.readFromRT( ) ) );
// Controller is inactive but let's put some data into buffer ( simulate callback when inactive )
command_msg = std::make_shared<std_msgs::msg::Float64MultiArray>( );
command_msg->data = {5.5, 6.6, 7.7};
controller_->rt_command_ptr_.writeFromNonRT( command_msg );
// command ptr should be available and message should be there - same check as in `update`
ASSERT_TRUE(
controller_->rt_command_ptr_.readFromNonRT( ) &&
*( controller_->rt_command_ptr_.readFromNonRT( ) ) );
ASSERT_TRUE(
controller_->rt_command_ptr_.readFromRT( ) && *( controller_->rt_command_ptr_.readFromRT( ) ) );
// Now activate again
node_state = controller_->get_node( )->activate( );
ASSERT_EQ( node_state.id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
// command ptr should be reset ( nullptr ) after activation - same check as in `update`
ASSERT_FALSE(
controller_->rt_command_ptr_.readFromNonRT( ) &&
*( controller_->rt_command_ptr_.readFromNonRT( ) ) );
ASSERT_FALSE(
controller_->rt_command_ptr_.readFromRT( ) && *( controller_->rt_command_ptr_.readFromRT( ) ) );
// update successful
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
// values should not change
ASSERT_EQ( joint_1_pos_cmd_.get_value( ), 10 );
ASSERT_EQ( joint_2_pos_cmd_.get_value( ), 20 );
ASSERT_EQ( joint_3_pos_cmd_.get_value( ), 30 );
// set commands again
controller_->rt_command_ptr_.writeFromNonRT( command_msg );
// update successful
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
// check command in handle was set
ASSERT_EQ( joint_1_pos_cmd_.get_value( ), 5.5 );
ASSERT_EQ( joint_2_pos_cmd_.get_value( ), 6.6 );
ASSERT_EQ( joint_3_pos_cmd_.get_value( ), 7.7 );
}
// Copyright 2020 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TEST_FORWARD_COMMAND_CONTROLLER_HPP_
#define TEST_FORWARD_COMMAND_CONTROLLER_HPP_
#include <memory>
#include <string>
#include <vector>
#include "gmock/gmock.h"
#include "forward_command_controller/forward_command_controller.hpp"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
using hardware_interface::CommandInterface;
using hardware_interface::HW_IF_POSITION;
// subclassing and friending so we can access member variables
32 class FriendForwardCommandController : public forward_command_controller::ForwardCommandController
{
34 FRIEND_TEST( ForwardCommandControllerTest, JointsParameterNotSet );
35 FRIEND_TEST( ForwardCommandControllerTest, InterfaceParameterNotSet );
36 FRIEND_TEST( ForwardCommandControllerTest, JointsParameterIsEmpty );
37 FRIEND_TEST( ForwardCommandControllerTest, InterfaceParameterEmpty );
38 FRIEND_TEST( ForwardCommandControllerTest, ConfigureParamsSuccess );
40 FRIEND_TEST( ForwardCommandControllerTest, ActivateWithWrongJointsNamesFails );
41 FRIEND_TEST( ForwardCommandControllerTest, ActivateWithWrongInterfaceNameFails );
42 FRIEND_TEST( ForwardCommandControllerTest, ActivateSuccess );
43 FRIEND_TEST( ForwardCommandControllerTest, CommandSuccessTest );
44 FRIEND_TEST( ForwardCommandControllerTest, WrongCommandCheckTest );
45 FRIEND_TEST( ForwardCommandControllerTest, NoCommandCheckTest );
46 FRIEND_TEST( ForwardCommandControllerTest, CommandCallbackTest );
47 FRIEND_TEST( ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess );
};
50 class ForwardCommandControllerTest : public ::testing::Test
{
public:
53 static void SetUpTestCase( );
54 static void TearDownTestCase( );
56 void SetUp( );
57 void TearDown( );
59 void SetUpController( );
60 void SetUpHandles( );
protected:
63 std::unique_ptr<FriendForwardCommandController> controller_;
// dummy joint state values used for tests
66 const std::vector<std::string> joint_names_ = {"joint1", "joint2", "joint3"};
67 std::vector<double> joint_commands_ = {1.1, 2.1, 3.1};
CommandInterface joint_1_pos_cmd_{joint_names_[0], HW_IF_POSITION, &joint_commands_[0]};
70 CommandInterface joint_2_pos_cmd_{joint_names_[1], HW_IF_POSITION, &joint_commands_[1]};
CommandInterface joint_3_pos_cmd_{joint_names_[2], HW_IF_POSITION, &joint_commands_[2]};
};
#endif // TEST_FORWARD_COMMAND_CONTROLLER_HPP_
1 // Copyright 2020 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gmock/gmock.h>
#include <memory>
#include "controller_manager/controller_manager.hpp"
#include "hardware_interface/resource_manager.hpp"
#include "rclcpp/executor.hpp"
#include "rclcpp/executors/single_threaded_executor.hpp"
#include "rclcpp/utilities.hpp"
#include "ros2_control_test_assets/descriptions.hpp"
25 TEST( TestLoadForwardCommandController, load_controller )
{
rclcpp::init( 0, nullptr );
std::shared_ptr<rclcpp::Executor> executor =
std::make_shared<rclcpp::executors::SingleThreadedExecutor>( );
controller_manager::ControllerManager cm(
std::make_unique<hardware_interface::ResourceManager>(
ros2_control_test_assets::minimal_robot_urdf ),
executor, "test_controller_manager" );
ASSERT_NO_THROW( cm.load_controller(
"test_forward_command_controller", "forward_command_controller/ForwardCommandController" ) );
rclcpp::shutdown( );
}
1 // Copyright ( c ) 2021, PickNik, Inc.
// Copyright ( c ) 2021, Stogl Robotics Consulting UG ( haftungsbeschränkt ) ( template )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gmock/gmock.h>
#include <memory>
#include "controller_manager/controller_manager.hpp"
#include "hardware_interface/resource_manager.hpp"
#include "rclcpp/executor.hpp"
#include "rclcpp/executors/single_threaded_executor.hpp"
#include "rclcpp/utilities.hpp"
#include "ros2_control_test_assets/descriptions.hpp"
26 TEST( TestLoadMultiInterfaceForwardController, load_controller )
{
rclcpp::init( 0, nullptr );
std::shared_ptr<rclcpp::Executor> executor =
std::make_shared<rclcpp::executors::SingleThreadedExecutor>( );
controller_manager::ControllerManager cm(
std::make_unique<hardware_interface::ResourceManager>(
ros2_control_test_assets::minimal_robot_urdf ),
executor, "test_controller_manager" );
ASSERT_NO_THROW( cm.load_controller(
"test_forward_command_controller",
"forward_command_controller/MultiInterfaceForwardCommandController" ) );
}
1 // Copyright ( c ) 2021, PickNik, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
/// \authors: Jack Center, Denis Stogl
#include <functional>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "gmock/gmock.h"
#include "test_multi_interface_forward_command_controller.hpp"
#include "forward_command_controller/multi_interface_forward_command_controller.hpp"
#include "hardware_interface/loaned_command_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/qos.hpp"
#include "rclcpp/subscription.hpp"
#include "rclcpp/utilities.hpp"
#include "rclcpp/wait_set.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
using hardware_interface::LoanedCommandInterface;
namespace
{
42 rclcpp::WaitResultKind wait_for( rclcpp::SubscriptionBase::SharedPtr subscription )
{
rclcpp::WaitSet wait_set;
wait_set.add_subscription( subscription );
const auto timeout = std::chrono::seconds( 10 );
return wait_set.wait( timeout ).kind( );
}
} // namespace
51 void MultiInterfaceForwardCommandControllerTest::SetUpTestCase( ) { rclcpp::init( 0, nullptr ); }
53 void MultiInterfaceForwardCommandControllerTest::TearDownTestCase( ) { rclcpp::shutdown( ); }
55 void MultiInterfaceForwardCommandControllerTest::SetUp( )
{
// initialize controller
controller_ = std::make_unique<FriendMultiInterfaceForwardCommandController>( );
}
61 void MultiInterfaceForwardCommandControllerTest::TearDown( ) { controller_.reset( nullptr ); }
63 void MultiInterfaceForwardCommandControllerTest::SetUpController( bool set_params_and_activate )
{
const auto result = controller_->init( "multi_interface_forward_command_controller" );
ASSERT_EQ( result, controller_interface::return_type::OK );
std::vector<LoanedCommandInterface> command_ifs;
command_ifs.emplace_back( joint_1_pos_cmd_ );
command_ifs.emplace_back( joint_1_vel_cmd_ );
command_ifs.emplace_back( joint_1_eff_cmd_ );
controller_->assign_interfaces( std::move( command_ifs ), {} );
if ( set_params_and_activate )
{
SetParametersAndActivateController( );
}
}
80 void MultiInterfaceForwardCommandControllerTest::SetParametersAndActivateController( )
{
controller_->get_node( )->set_parameter( {"joint", "joint1"} );
controller_->get_node( )->set_parameter(
{"interface_names", std::vector<std::string>{"position", "velocity", "effort"}} );
auto node_state = controller_->get_node( )->configure( );
ASSERT_EQ( node_state.id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
node_state = controller_->get_node( )->activate( );
ASSERT_EQ( node_state.id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
}
92 TEST_F( MultiInterfaceForwardCommandControllerTest, JointsParameterNotSet )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"interface_names", std::vector<std::string>( )} );
// configure failed, 'joint' parameter not set
ASSERT_EQ(
controller_->on_configure( rclcpp_lifecycle::State( ) ),
controller_interface::CallbackReturn::ERROR );
}
103 TEST_F( MultiInterfaceForwardCommandControllerTest, InterfaceParameterNotSet )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joint", ""} );
// configure failed, 'interface_names' parameter not set
ASSERT_EQ(
controller_->on_configure( rclcpp_lifecycle::State( ) ),
controller_interface::CallbackReturn::ERROR );
}
114 TEST_F( MultiInterfaceForwardCommandControllerTest, JointsParameterIsEmpty )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joint", ""} );
controller_->get_node( )->set_parameter( {"interface_names", std::vector<std::string>( )} );
// configure failed, 'joint' is empty
ASSERT_EQ(
controller_->on_configure( rclcpp_lifecycle::State( ) ),
controller_interface::CallbackReturn::ERROR );
}
127 TEST_F( MultiInterfaceForwardCommandControllerTest, InterfaceParameterEmpty )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joint", "joint1"} );
controller_->get_node( )->set_parameter( {"interface_names", std::vector<std::string>( )} );
// configure failed, 'interface_name' is empty
ASSERT_EQ(
controller_->on_configure( rclcpp_lifecycle::State( ) ),
controller_interface::CallbackReturn::ERROR );
}
139 TEST_F( MultiInterfaceForwardCommandControllerTest, ConfigureParamsSuccess )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joint", "joint1"} );
controller_->get_node( )->set_parameter(
{"interface_names", std::vector<std::string>{"position", "velocity", "effort"}} );
// configure successful
ASSERT_EQ(
controller_->on_configure( rclcpp_lifecycle::State( ) ),
controller_interface::CallbackReturn::SUCCESS );
}
153 TEST_F( MultiInterfaceForwardCommandControllerTest, ActivateWithWrongJointsNamesFails )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joint", "joint2"} );
controller_->get_node( )->set_parameter(
{"interface_names", std::vector<std::string>{"position", "velocity", "effort"}} );
// activate failed, 'joint2' is not a valid joint name for the hardware
ASSERT_EQ(
controller_->on_configure( rclcpp_lifecycle::State( ) ),
controller_interface::CallbackReturn::SUCCESS );
ASSERT_EQ(
controller_->on_activate( rclcpp_lifecycle::State( ) ),
controller_interface::CallbackReturn::ERROR );
}
170 TEST_F( MultiInterfaceForwardCommandControllerTest, ActivateWithWrongInterfaceNameFails )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joint", "joint1"} );
controller_->get_node( )->set_parameter(
{"interface_names", std::vector<std::string>{"position", "velocity", "acceleration"}} );
// activate failed, 'acceleration' is not a registered interface for `joint1`
ASSERT_EQ(
controller_->on_configure( rclcpp_lifecycle::State( ) ),
controller_interface::CallbackReturn::SUCCESS );
ASSERT_EQ(
controller_->on_activate( rclcpp_lifecycle::State( ) ),
controller_interface::CallbackReturn::ERROR );
}
187 TEST_F( MultiInterfaceForwardCommandControllerTest, ActivateSuccess )
{
SetUpController( true );
// check joint commands are the default ones
ASSERT_EQ( joint_1_pos_cmd_.get_value( ), 1.1 );
ASSERT_EQ( joint_1_vel_cmd_.get_value( ), 2.1 );
ASSERT_EQ( joint_1_eff_cmd_.get_value( ), 3.1 );
}
197 TEST_F( MultiInterfaceForwardCommandControllerTest, CommandSuccessTest )
{
SetUpController( true );
// send command
auto command_ptr = std::make_shared<forward_command_controller::CmdType>( );
command_ptr->data = {10.0, 20.0, 30.0};
controller_->rt_command_ptr_.writeFromNonRT( command_ptr );
// update successful, command received
ASSERT_EQ(
controller_->update( rclcpp::Time( 0.1 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
// check command in handle was set
ASSERT_EQ( joint_1_pos_cmd_.get_value( ), 10.0 );
ASSERT_EQ( joint_1_vel_cmd_.get_value( ), 20.0 );
ASSERT_EQ( joint_1_eff_cmd_.get_value( ), 30.0 );
}
217 TEST_F( MultiInterfaceForwardCommandControllerTest, NoCommandCheckTest )
{
SetUpController( true );
// update successful, no command received yet
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
// check joint commands are still the default ones
ASSERT_EQ( joint_1_pos_cmd_.get_value( ), 1.1 );
ASSERT_EQ( joint_1_vel_cmd_.get_value( ), 2.1 );
ASSERT_EQ( joint_1_eff_cmd_.get_value( ), 3.1 );
}
232 TEST_F( MultiInterfaceForwardCommandControllerTest, WrongCommandCheckTest )
{
SetUpController( true );
// send command with wrong number of joints
auto command_ptr = std::make_shared<forward_command_controller::CmdType>( );
command_ptr->data = {10.0, 20.0};
controller_->rt_command_ptr_.writeFromNonRT( command_ptr );
// update failed, command size does not match number of joints
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::ERROR );
// check joint commands are still the default ones
ASSERT_EQ( joint_1_pos_cmd_.get_value( ), 1.1 );
ASSERT_EQ( joint_1_vel_cmd_.get_value( ), 2.1 );
ASSERT_EQ( joint_1_eff_cmd_.get_value( ), 3.1 );
}
252 TEST_F( MultiInterfaceForwardCommandControllerTest, CommandCallbackTest )
{
SetUpController( true );
// send a new command
rclcpp::Node test_node( "test_node" );
auto command_pub = test_node.create_publisher<std_msgs::msg::Float64MultiArray>(
std::string( controller_->get_node( )->get_name( ) ) + "/commands", rclcpp::SystemDefaultsQoS( ) );
std_msgs::msg::Float64MultiArray command_msg;
command_msg.data = {10.0, 20.0, 30.0};
command_pub->publish( command_msg );
// wait for command message to be passed
ASSERT_EQ( wait_for( controller_->joints_command_subscriber_ ), rclcpp::WaitResultKind::Ready );
// process callbacks
rclcpp::spin_some( controller_->get_node( )->get_node_base_interface( ) );
// update successful
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
// check command in handle was set
ASSERT_EQ( joint_1_pos_cmd_.get_value( ), 10.0 );
ASSERT_EQ( joint_1_vel_cmd_.get_value( ), 20.0 );
ASSERT_EQ( joint_1_eff_cmd_.get_value( ), 30.0 );
}
281 TEST_F( MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess )
{
SetUpController( true );
// send command
auto command_ptr = std::make_shared<forward_command_controller::CmdType>( );
command_ptr->data = {10.0, 20.0, 30.0};
controller_->rt_command_ptr_.writeFromNonRT( command_ptr );
// update successful, command received
ASSERT_EQ(
controller_->update( rclcpp::Time( 0.1 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
// check command in handle was set
ASSERT_EQ( joint_1_pos_cmd_.get_value( ), 10.0 );
ASSERT_EQ( joint_1_vel_cmd_.get_value( ), 20.0 );
ASSERT_EQ( joint_1_eff_cmd_.get_value( ), 30.0 );
auto node_state = controller_->get_node( )->deactivate( );
ASSERT_EQ( node_state.id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
// command ptr should be reset ( nullptr ) after deactivation - same check as in `update`
ASSERT_FALSE(
controller_->rt_command_ptr_.readFromNonRT( ) &&
*( controller_->rt_command_ptr_.readFromNonRT( ) ) );
ASSERT_FALSE(
controller_->rt_command_ptr_.readFromRT( ) && *( controller_->rt_command_ptr_.readFromRT( ) ) );
// Controller is inactive but let's put some data into buffer ( simulate callback when inactive )
auto command_msg = std::make_shared<std_msgs::msg::Float64MultiArray>( );
command_msg->data = {5.5, 6.6, 7.7};
controller_->rt_command_ptr_.writeFromNonRT( command_msg );
// command ptr should be available and message should be there - same check as in `update`
ASSERT_TRUE(
controller_->rt_command_ptr_.readFromNonRT( ) &&
*( controller_->rt_command_ptr_.readFromNonRT( ) ) );
ASSERT_TRUE(
controller_->rt_command_ptr_.readFromRT( ) && *( controller_->rt_command_ptr_.readFromRT( ) ) );
// Now activate again
node_state = controller_->get_node( )->activate( );
ASSERT_EQ( node_state.id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
// command ptr should be reset ( nullptr ) after activation - same check as in `update`
ASSERT_FALSE(
controller_->rt_command_ptr_.readFromNonRT( ) &&
*( controller_->rt_command_ptr_.readFromNonRT( ) ) );
ASSERT_FALSE(
controller_->rt_command_ptr_.readFromRT( ) && *( controller_->rt_command_ptr_.readFromRT( ) ) );
// update successful
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
// values should not change
ASSERT_EQ( joint_1_pos_cmd_.get_value( ), 10.0 );
ASSERT_EQ( joint_1_vel_cmd_.get_value( ), 20.0 );
ASSERT_EQ( joint_1_eff_cmd_.get_value( ), 30.0 );
// set commands again
controller_->rt_command_ptr_.writeFromNonRT( command_msg );
// update successful
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
// check command in handle was set
ASSERT_EQ( joint_1_pos_cmd_.get_value( ), 5.5 );
ASSERT_EQ( joint_1_vel_cmd_.get_value( ), 6.6 );
ASSERT_EQ( joint_1_eff_cmd_.get_value( ), 7.7 );
}
// Copyright ( c ) 2021, PickNik, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
/// \authors: Jack Center, Denis Stogl
#ifndef TEST_MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_
#define TEST_MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_
#include <memory>
#include <string>
#include <vector>
#include "gmock/gmock.h"
#include "forward_command_controller/multi_interface_forward_command_controller.hpp"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
using hardware_interface::CommandInterface;
using hardware_interface::HW_IF_EFFORT;
using hardware_interface::HW_IF_POSITION;
using hardware_interface::HW_IF_VELOCITY;
// subclassing and friending so we can access member variables
36 class FriendMultiInterfaceForwardCommandController
37 : public forward_command_controller::MultiInterfaceForwardCommandController
{
39 FRIEND_TEST( MultiInterfaceForwardCommandControllerTest, JointsParameterNotSet );
40 FRIEND_TEST( MultiInterfaceForwardCommandControllerTest, InterfaceParameterNotSet );
41 FRIEND_TEST( MultiInterfaceForwardCommandControllerTest, JointsParameterIsEmpty );
42 FRIEND_TEST( MultiInterfaceForwardCommandControllerTest, InterfaceParameterEmpty );
43 FRIEND_TEST( MultiInterfaceForwardCommandControllerTest, ConfigureParamsSuccess );
45 FRIEND_TEST( MultiInterfaceForwardCommandControllerTest, ActivateWithWrongJointsNamesFails );
46 FRIEND_TEST( MultiInterfaceForwardCommandControllerTest, ActivateWithWrongInterfaceNameFails );
47 FRIEND_TEST( MultiInterfaceForwardCommandControllerTest, ActivateSuccess );
48 FRIEND_TEST( MultiInterfaceForwardCommandControllerTest, CommandSuccessTest );
49 FRIEND_TEST( MultiInterfaceForwardCommandControllerTest, WrongCommandCheckTest );
50 FRIEND_TEST( MultiInterfaceForwardCommandControllerTest, NoCommandCheckTest );
51 FRIEND_TEST( MultiInterfaceForwardCommandControllerTest, CommandCallbackTest );
52 FRIEND_TEST( MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess );
};
55 class MultiInterfaceForwardCommandControllerTest : public ::testing::Test
{
public:
58 static void SetUpTestCase( );
59 static void TearDownTestCase( );
61 void SetUp( );
62 void TearDown( );
64 void SetUpController( bool set_params_and_activate = false );
65 void SetParametersAndActivateController( );
protected:
68 std::unique_ptr<FriendMultiInterfaceForwardCommandController> controller_;
// dummy joint state value used for tests
71 const std::string joint_name_ = "joint1";
double pos_cmd_ = 1.1;
double vel_cmd_ = 2.1;
double eff_cmd_ = 3.1;
CommandInterface joint_1_pos_cmd_{joint_name_, HW_IF_POSITION, &pos_cmd_};
78 CommandInterface joint_1_vel_cmd_{joint_name_, HW_IF_VELOCITY, &vel_cmd_};
CommandInterface joint_1_eff_cmd_{joint_name_, HW_IF_EFFORT, &eff_cmd_};
};
#endif // TEST_MULTI_INTERFACE_FORWARD_COMMAND_CONTROLLER_HPP_
1 // Copyright 2014, SRI International
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/// \author Sachin Chitta, Adolfo Rodriguez Tsouroukdissian
#ifndef GRIPPER_CONTROLLERS__GRIPPER_ACTION_CONTROLLER_HPP_
#define GRIPPER_CONTROLLERS__GRIPPER_ACTION_CONTROLLER_HPP_
// C++ standard
#include <cassert>
#include <memory>
#include <stdexcept>
#include <string>
// TODO( JafarAbdi ): Remove experimental once the default standard is C++17
#include "experimental/optional"
// ROS
#include "rclcpp/rclcpp.hpp"
// ROS messages
#include "control_msgs/action/gripper_command.hpp"
// rclcpp_action
#include "rclcpp_action/create_server.hpp"
// ros_controls
#include "controller_interface/controller_interface.hpp"
#include "gripper_controllers/visibility_control.hpp"
#include "hardware_interface/loaned_command_interface.hpp"
#include "hardware_interface/loaned_state_interface.hpp"
#include "realtime_tools/realtime_buffer.h"
#include "realtime_tools/realtime_server_goal_handle.h"
// Project
#include "gripper_controllers/hardware_interface_adapter.hpp"
namespace gripper_action_controller
{
/**
* \brief Controller for executing a gripper command action for simple
* single-dof grippers.
*
* \tparam HardwareInterface Controller hardware interface. Currently \p
* hardware_interface::HW_IF_POSITION and \p
* hardware_interface::HW_IF_EFFORT are supported out-of-the-box.
*/
template <const char * HardwareInterface>
59 class GripperActionController : public controller_interface::ControllerInterface
{
public:
/**
* \brief Store position and max effort in struct to allow easier realtime
* buffer usage
*/
struct Commands
{
double position_; // Last commanded position
double max_effort_; // Max allowed effort
};
72 GRIPPER_ACTION_CONTROLLER_PUBLIC GripperActionController( );
/**
* @brief command_interface_configuration This controller requires the
* position command interfaces for the controlled joints
*/
GRIPPER_ACTION_CONTROLLER_PUBLIC
controller_interface::InterfaceConfiguration command_interface_configuration( ) const override;
/**
* @brief command_interface_configuration This controller requires the
* position and velocity state interfaces for the controlled joints
*/
GRIPPER_ACTION_CONTROLLER_PUBLIC
controller_interface::InterfaceConfiguration state_interface_configuration( ) const override;
GRIPPER_ACTION_CONTROLLER_PUBLIC
controller_interface::return_type update(
const rclcpp::Time & time, const rclcpp::Duration & period ) override;
GRIPPER_ACTION_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_init( ) override;
GRIPPER_ACTION_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_configure(
const rclcpp_lifecycle::State & previous_state ) override;
GRIPPER_ACTION_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_activate(
const rclcpp_lifecycle::State & previous_state ) override;
GRIPPER_ACTION_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_deactivate(
const rclcpp_lifecycle::State & previous_state ) override;
realtime_tools::RealtimeBuffer<Commands> command_;
// pre-allocated memory that is re-used to set the realtime buffer
Commands command_struct_, command_struct_rt_;
private:
using GripperCommandAction = control_msgs::action::GripperCommand;
using ActionServer = rclcpp_action::Server<GripperCommandAction>;
using ActionServerPtr = ActionServer::SharedPtr;
using GoalHandle = rclcpp_action::ServerGoalHandle<GripperCommandAction>;
using RealtimeGoalHandle =
realtime_tools::RealtimeServerGoalHandle<control_msgs::action::GripperCommand>;
using RealtimeGoalHandlePtr = std::shared_ptr<RealtimeGoalHandle>;
using HwIfaceAdapter = HardwareInterfaceAdapter<HardwareInterface>;
bool update_hold_position_;
bool verbose_ = false; ///< Hard coded verbose flag to help in debugging
std::string name_; ///< Controller name.
std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
joint_position_command_interface_;
std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
joint_position_state_interface_;
std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
joint_velocity_state_interface_;
std::string joint_name_; ///< Controlled joint names.
HwIfaceAdapter hw_iface_adapter_; ///< Adapts desired goal state to HW interface.
RealtimeGoalHandlePtr rt_active_goal_; ///< Currently active action goal, if any.
control_msgs::action::GripperCommand::Result::SharedPtr pre_alloc_result_;
rclcpp::Duration action_monitor_period_;
// ROS API
ActionServerPtr action_server_;
rclcpp::TimerBase::SharedPtr goal_handle_timer_;
rclcpp_action::GoalResponse goal_callback(
const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const GripperCommandAction::Goal> goal );
rclcpp_action::CancelResponse cancel_callback( const std::shared_ptr<GoalHandle> goal_handle );
void accepted_callback( std::shared_ptr<GoalHandle> goal_handle );
void preempt_active_goal( );
void set_hold_position( );
rclcpp::Time last_movement_time_ = rclcpp::Time( 0, 0, RCL_ROS_TIME ); ///< Store stall time
double computed_command_; ///< Computed command
double stall_timeout_,
stall_velocity_threshold_; ///< Stall related parameters
double default_max_effort_; ///< Max allowed effort
double goal_tolerance_;
bool allow_stalling_; ///< If gripper stalls when moving to goal is considered successful
/**
* \brief Check for success and publish appropriate result and feedback.
**/
void check_for_success(
const rclcpp::Time & time, double error_position, double current_position,
double current_velocity );
};
} // namespace gripper_action_controller
#include "gripper_controllers/gripper_action_controller_impl.hpp"
#endif // GRIPPER_CONTROLLERS__GRIPPER_ACTION_CONTROLLER_HPP_
1 // Copyright 2014, SRI International
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/// \author Sachin Chitta, Adolfo Rodriguez Tsouroukdissian, Stu Glaser
#ifndef GRIPPER_CONTROLLERS__GRIPPER_ACTION_CONTROLLER_IMPL_HPP_
#define GRIPPER_CONTROLLERS__GRIPPER_ACTION_CONTROLLER_IMPL_HPP_
#include "gripper_controllers/gripper_action_controller.hpp"
#include <memory>
#include <string>
namespace gripper_action_controller
{
template <const char * HardwareInterface>
28 void GripperActionController<HardwareInterface>::preempt_active_goal( )
{
// Cancels the currently active goal
if ( rt_active_goal_ )
{
// Marks the current goal as canceled
rt_active_goal_->setCanceled( std::make_shared<GripperCommandAction::Result>( ) );
rt_active_goal_.reset( );
}
}
template <const char * HardwareInterface>
40 controller_interface::CallbackReturn GripperActionController<HardwareInterface>::on_init( )
{
try
{
// with the lifecycle node being initialized, we can declare parameters
auto_declare<double>( "action_monitor_rate", 20.0 );
auto_declare<std::string>( "joint", joint_name_ );
auto_declare<double>( "goal_tolerance", 0.01 );
auto_declare<double>( "max_effort", 0.0 );
auto_declare<bool>( "allow_stalling", false );
auto_declare<double>( "stall_velocity_threshold", 0.001 );
auto_declare<double>( "stall_timeout", 1.0 );
}
catch ( const std::exception & e )
{
fprintf( stderr, "Exception thrown during init stage with message: %s \n", e.what( ) );
return controller_interface::CallbackReturn::ERROR;
}
return controller_interface::CallbackReturn::SUCCESS;
}
template <const char * HardwareInterface>
63 controller_interface::return_type GripperActionController<HardwareInterface>::update(
64 const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/ )
{
command_struct_rt_ = *( command_.readFromRT( ) );
const double current_position = joint_position_state_interface_->get( ).get_value( );
const double current_velocity = joint_velocity_state_interface_->get( ).get_value( );
const double error_position = command_struct_rt_.position_ - current_position;
const double error_velocity = -current_velocity;
check_for_success( get_node( )->now( ), error_position, current_position, current_velocity );
// Hardware interface adapter: Generate and send commands
computed_command_ = hw_iface_adapter_.updateCommand(
command_struct_rt_.position_, 0.0, error_position, error_velocity,
command_struct_rt_.max_effort_ );
return controller_interface::return_type::OK;
}
template <const char * HardwareInterface>
84 rclcpp_action::GoalResponse GripperActionController<HardwareInterface>::goal_callback(
85 const rclcpp_action::GoalUUID &, std::shared_ptr<const GripperCommandAction::Goal> )
{
RCLCPP_INFO( get_node( )->get_logger( ), "Received & accepted new action goal" );
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
template <const char * HardwareInterface>
92 void GripperActionController<HardwareInterface>::accepted_callback(
93 std::shared_ptr<GoalHandle> goal_handle ) // Try to update goal
{
{
auto rt_goal = std::make_shared<RealtimeGoalHandle>( goal_handle );
// Accept new goal
preempt_active_goal( );
// This is the non-realtime command_struct
// We use command_ for sharing
command_struct_.position_ = goal_handle->get_goal( )->command.position;
command_struct_.max_effort_ = goal_handle->get_goal( )->command.max_effort;
command_.writeFromNonRT( command_struct_ );
pre_alloc_result_->reached_goal = false;
pre_alloc_result_->stalled = false;
last_movement_time_ = get_node( )->now( );
rt_active_goal_ = rt_goal;
rt_active_goal_->execute( );
}
// Setup goal status checking timer
goal_handle_timer_ = get_node( )->create_wall_timer(
action_monitor_period_.to_chrono<std::chrono::seconds>( ),
std::bind( &RealtimeGoalHandle::runNonRealtime, rt_active_goal_ ) );
}
template <const char * HardwareInterface>
121 rclcpp_action::CancelResponse GripperActionController<HardwareInterface>::cancel_callback(
122 const std::shared_ptr<GoalHandle> goal_handle )
{
RCLCPP_INFO( get_node( )->get_logger( ), "Got request to cancel goal" );
// Check that cancel request refers to currently active goal ( if any )
if ( rt_active_goal_ && rt_active_goal_->gh_ == goal_handle )
{
// Enter hold current position mode
set_hold_position( );
RCLCPP_INFO(
get_node( )->get_logger( ), "Canceling active action goal because cancel callback received." );
// Mark the current goal as canceled
auto action_res = std::make_shared<GripperCommandAction::Result>( );
rt_active_goal_->setCanceled( action_res );
// Reset current goal
rt_active_goal_.reset( );
}
return rclcpp_action::CancelResponse::ACCEPT;
}
template <const char * HardwareInterface>
145 void GripperActionController<HardwareInterface>::set_hold_position( )
{
command_struct_.position_ = joint_position_state_interface_->get( ).get_value( );
command_struct_.max_effort_ = default_max_effort_;
command_.writeFromNonRT( command_struct_ );
}
template <const char * HardwareInterface>
153 void GripperActionController<HardwareInterface>::check_for_success(
154 const rclcpp::Time & time, double error_position, double current_position,
double current_velocity )
{
if ( !rt_active_goal_ )
{
return;
}
if ( fabs( error_position ) < goal_tolerance_ )
{
pre_alloc_result_->effort = computed_command_;
pre_alloc_result_->position = current_position;
pre_alloc_result_->reached_goal = true;
pre_alloc_result_->stalled = false;
RCLCPP_DEBUG( get_node( )->get_logger( ), "Successfully moved to goal." );
rt_active_goal_->setSucceeded( pre_alloc_result_ );
rt_active_goal_.reset( );
}
else
{
if ( fabs( current_velocity ) > stall_velocity_threshold_ )
{
last_movement_time_ = time;
}
else if ( ( time - last_movement_time_ ).seconds( ) > stall_timeout_ )
{
pre_alloc_result_->effort = computed_command_;
pre_alloc_result_->position = current_position;
pre_alloc_result_->reached_goal = false;
pre_alloc_result_->stalled = true;
if( allow_stalling_ )
{
RCLCPP_DEBUG( get_node( )->get_logger( ), "Stall detected moving to goal. Returning success." );
rt_active_goal_->setSucceeded( pre_alloc_result_ );
}
else
{
RCLCPP_DEBUG( get_node( )->get_logger( ), "Stall detected moving to goal. Aborting action!" );
rt_active_goal_->setAborted( pre_alloc_result_ );
}
rt_active_goal_.reset( );
}
}
}
template <const char * HardwareInterface>
200 controller_interface::CallbackReturn GripperActionController<HardwareInterface>::on_configure(
201 const rclcpp_lifecycle::State & )
{
const auto logger = get_node( )->get_logger( );
// Action status checking update rate
const auto action_monitor_rate = get_node( )->get_parameter( "action_monitor_rate" ).as_double( );
action_monitor_period_ = rclcpp::Duration::from_seconds(
1.0 / get_node( )->get_parameter( "action_monitor_rate" ).as_double( ) );
RCLCPP_INFO_STREAM(
logger, "Action status changes will be monitored at " << action_monitor_rate << "Hz." );
// Controlled joint
joint_name_ = get_node( )->get_parameter( "joint" ).as_string( );
if ( joint_name_.empty( ) )
{
RCLCPP_ERROR( logger, "Could not find joint name on param server" );
return controller_interface::CallbackReturn::ERROR;
}
// Default tolerances
goal_tolerance_ = get_node( )->get_parameter( "goal_tolerance" ).as_double( );
goal_tolerance_ = fabs( goal_tolerance_ );
// Max allowable effort
default_max_effort_ = get_node( )->get_parameter( "max_effort" ).as_double( );
default_max_effort_ = fabs( default_max_effort_ );
// Allow stalling will make the action server return success if the
// gripper stalls when moving to the goal
allow_stalling_ = get_node( )->get_parameter( "allow_stalling" ).as_bool( );
// Stall - stall velocity threshold, stall timeout
stall_velocity_threshold_ = get_node( )->get_parameter( "stall_velocity_threshold" ).as_double( );
stall_timeout_ = get_node( )->get_parameter( "stall_timeout" ).as_double( );
return controller_interface::CallbackReturn::SUCCESS;
}
template <const char * HardwareInterface>
236 controller_interface::CallbackReturn GripperActionController<HardwareInterface>::on_activate(
237 const rclcpp_lifecycle::State & )
{
auto position_command_interface_it = std::find_if(
command_interfaces_.begin( ), command_interfaces_.end( ),
[]( const hardware_interface::LoanedCommandInterface & command_interface ) {
return command_interface.get_interface_name( ) == hardware_interface::HW_IF_POSITION;
} );
if ( position_command_interface_it == command_interfaces_.end( ) )
{
RCLCPP_ERROR( get_node( )->get_logger( ), "Expected 1 position command interface" );
return controller_interface::CallbackReturn::ERROR;
}
if ( position_command_interface_it->get_prefix_name( ) != joint_name_ )
{
RCLCPP_ERROR_STREAM(
get_node( )->get_logger( ), "Position command interface is different than joint name `"
<< position_command_interface_it->get_prefix_name( ) << "` != `"
<< joint_name_ << "`" );
return controller_interface::CallbackReturn::ERROR;
}
const auto position_state_interface_it = std::find_if(
state_interfaces_.begin( ), state_interfaces_.end( ),
[]( const hardware_interface::LoanedStateInterface & state_interface ) {
return state_interface.get_interface_name( ) == hardware_interface::HW_IF_POSITION;
} );
if ( position_state_interface_it == state_interfaces_.end( ) )
{
RCLCPP_ERROR( get_node( )->get_logger( ), "Expected 1 position state interface" );
return controller_interface::CallbackReturn::ERROR;
}
if ( position_state_interface_it->get_prefix_name( ) != joint_name_ )
{
RCLCPP_ERROR_STREAM(
get_node( )->get_logger( ), "Position state interface is different than joint name `"
<< position_state_interface_it->get_prefix_name( ) << "` != `"
<< joint_name_ << "`" );
return controller_interface::CallbackReturn::ERROR;
}
const auto velocity_state_interface_it = std::find_if(
state_interfaces_.begin( ), state_interfaces_.end( ),
[]( const hardware_interface::LoanedStateInterface & state_interface ) {
return state_interface.get_interface_name( ) == hardware_interface::HW_IF_VELOCITY;
} );
if ( velocity_state_interface_it == state_interfaces_.end( ) )
{
RCLCPP_ERROR( get_node( )->get_logger( ), "Expected 1 velocity state interface" );
return controller_interface::CallbackReturn::ERROR;
}
if ( velocity_state_interface_it->get_prefix_name( ) != joint_name_ )
{
RCLCPP_ERROR_STREAM(
get_node( )->get_logger( ), "Velocity command interface is different than joint name `"
<< velocity_state_interface_it->get_prefix_name( ) << "` != `"
<< joint_name_ << "`" );
return controller_interface::CallbackReturn::ERROR;
}
joint_position_command_interface_ = *position_command_interface_it;
joint_position_state_interface_ = *position_state_interface_it;
joint_velocity_state_interface_ = *velocity_state_interface_it;
// Hardware interface adapter
hw_iface_adapter_.init( joint_position_command_interface_, get_node( ) );
// Command - non RT version
command_struct_.position_ = joint_position_state_interface_->get( ).get_value( );
command_struct_.max_effort_ = default_max_effort_;
// Result
pre_alloc_result_ = std::make_shared<control_msgs::action::GripperCommand::Result>( );
pre_alloc_result_->position = command_struct_.position_;
pre_alloc_result_->reached_goal = false;
pre_alloc_result_->stalled = false;
// Action interface
action_server_ = rclcpp_action::create_server<control_msgs::action::GripperCommand>(
get_node( ), "~/gripper_cmd",
std::bind(
&GripperActionController::goal_callback, this, std::placeholders::_1, std::placeholders::_2 ),
std::bind( &GripperActionController::cancel_callback, this, std::placeholders::_1 ),
std::bind( &GripperActionController::accepted_callback, this, std::placeholders::_1 ) );
return controller_interface::CallbackReturn::SUCCESS;
}
template <const char * HardwareInterface>
323 controller_interface::CallbackReturn GripperActionController<HardwareInterface>::on_deactivate(
324 const rclcpp_lifecycle::State & )
{
joint_position_command_interface_ = std::experimental::nullopt;
joint_position_state_interface_ = std::experimental::nullopt;
joint_velocity_state_interface_ = std::experimental::nullopt;
release_interfaces( );
return controller_interface::CallbackReturn::SUCCESS;
}
template <const char * HardwareInterface>
controller_interface::InterfaceConfiguration
335 GripperActionController<HardwareInterface>::command_interface_configuration( ) const
{
return {
controller_interface::interface_configuration_type::INDIVIDUAL,
{joint_name_ + "/" + hardware_interface::HW_IF_POSITION}};
}
template <const char * HardwareInterface>
controller_interface::InterfaceConfiguration
344 GripperActionController<HardwareInterface>::state_interface_configuration( ) const
{
return {
controller_interface::interface_configuration_type::INDIVIDUAL,
{joint_name_ + "/" + hardware_interface::HW_IF_POSITION,
joint_name_ + "/" + hardware_interface::HW_IF_VELOCITY}};
}
template <const char * HardwareInterface>
353 GripperActionController<HardwareInterface>::GripperActionController( )
: controller_interface::ControllerInterface( ),
action_monitor_period_( rclcpp::Duration::from_seconds( 0 ) )
{
}
} // namespace gripper_action_controller
#endif // GRIPPER_CONTROLLERS__GRIPPER_ACTION_CONTROLLER_IMPL_HPP_
// Copyright 2014, SRI International
// Copyright 2013, PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/// \author Sachin Chitta, Adolfo Rodriguez Tsouroukdissian
#ifndef GRIPPER_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER_HPP_
#define GRIPPER_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER_HPP_
#include <algorithm>
#include <cassert>
#include <memory>
#include <string>
#include <vector>
// TODO( JafarAbdi ): Remove experimental once the default standard is C++17
#include "experimental/optional"
#include "control_toolbox/pid.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
/**
* \brief Helper class to simplify integrating the GripperActionController with
* different hardware interfaces.
*
* The GripperActionController outputs position while
* it is supposed to work with either position or effort commands.
*
*/
template <const char * HardwareInterface>
43 class HardwareInterfaceAdapter
{
public:
46 bool init(
std::experimental::optional<
std::reference_wrapper<hardware_interface::LoanedCommandInterface>> /* joint_handle */,
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & /* node */ )
{
return false;
}
54 void starting( const rclcpp::Time & /* time */ ) {}
55 void stopping( const rclcpp::Time & /* time */ ) {}
57 double updateCommand(
double /* desired_position */, double /* desired_velocity */, double /* error_position */,
double /* error_velocity */, double /* max_allowed_effort */ )
{
return 0.0;
}
};
/**
* \brief Adapter for a position-controlled hardware interface. Forwards desired
* positions as commands.
*/
template <>
70 class HardwareInterfaceAdapter<hardware_interface::HW_IF_POSITION>
{
public:
73 bool init(
std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
joint_handle,
const rclcpp_lifecycle::LifecycleNode::SharedPtr & /* node */ )
{
joint_handle_ = joint_handle;
return true;
}
82 void starting( const rclcpp::Time & /* time */ ) {}
83 void stopping( const rclcpp::Time & /* time */ ) {}
85 double updateCommand(
double desired_position, double /* desired_velocity */, double /* error_position */,
double /* error_velocity */, double max_allowed_effort )
{
// Forward desired position to command
joint_handle_->get( ).set_value( desired_position );
return max_allowed_effort;
}
private:
std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
joint_handle_;
};
/**
* \brief Adapter for an effort-controlled hardware interface. Maps position and
* velocity errors to effort commands through a position PID loop.
*
* The following is an example configuration of a controller that uses this
* adapter. Notice the \p gains entry: \code gripper_controller: type:
* "gripper_action_controller/GripperActionController" joints: gripper_joint
* goal_tolerance: 0.01
* stalled_velocity_threshold: 0.01
* stall_timeout: 0.2
* gains:
* gripper_joint: {p: 200, d: 1, i: 5, i_clamp: 1}
* \endcode
*/
template <>
114 class HardwareInterfaceAdapter<hardware_interface::HW_IF_EFFORT>
{
public:
template <typename ParameterT>
auto auto_declare(
const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node, const std::string & name,
const ParameterT & default_value )
{
if ( !node->has_parameter( name ) )
{
return node->declare_parameter<ParameterT>( name, default_value );
}
else
{
return node->get_parameter( name ).get_value<ParameterT>( );
}
}
bool init(
std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
joint_handle,
const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node )
{
joint_handle_ = joint_handle;
// Init PID gains from ROS parameter server
const std::string prefix = "gains." + joint_handle_->get( ).get_name( );
const auto k_p = auto_declare<double>( node, prefix + ".p", 0.0 );
const auto k_i = auto_declare<double>( node, prefix + ".i", 0.0 );
const auto k_d = auto_declare<double>( node, prefix + ".d", 0.0 );
const auto i_clamp = auto_declare<double>( node, prefix + ".i_clamp", 0.0 );
// Initialize PID
pid_ = std::make_shared<control_toolbox::Pid>( k_p, k_i, k_d, i_clamp, -i_clamp );
return true;
}
void starting( const rclcpp::Time & /* time */ )
{
if ( !joint_handle_ )
{
return;
}
// Reset PIDs, zero effort commands
pid_->reset( );
joint_handle_->get( ).set_value( 0.0 );
}
void stopping( const rclcpp::Time & /* time */ ) {}
double updateCommand(
double /* desired_position */, double /* desired_velocity */, double error_position,
double error_velocity, double max_allowed_effort )
{
// Preconditions
if ( !joint_handle_ )
{
return 0.0;
}
// Time since the last call to update
const auto period = std::chrono::steady_clock::now( ) - last_update_time_;
// Update PIDs
double command = pid_->computeCommand( error_position, error_velocity, period.count( ) );
command = std::min<double>(
fabs( max_allowed_effort ), std::max<double>( -fabs( max_allowed_effort ), command ) );
joint_handle_->get( ).set_value( command );
last_update_time_ = std::chrono::steady_clock::now( );
return command;
}
private:
using PidPtr = std::shared_ptr<control_toolbox::Pid>;
PidPtr pid_;
std::experimental::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
joint_handle_;
std::chrono::steady_clock::time_point last_update_time_;
};
#endif // GRIPPER_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER_HPP_
1 // Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/* This header must be included by all rclcpp headers which declare symbols
* which are defined in the rclcpp library. When not building the rclcpp
* library, i.e. when using the headers in other package's code, the contents
* of this header change the visibility of certain symbols which the rclcpp
* library cannot have, but the consuming code must have inorder to link.
*/
#ifndef GRIPPER_CONTROLLERS__VISIBILITY_CONTROL_HPP_
#define GRIPPER_CONTROLLERS__VISIBILITY_CONTROL_HPP_
// This logic was borrowed ( then namespaced ) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define GRIPPER_ACTION_CONTROLLER_EXPORT __attribute__( ( dllexport ) )
#define GRIPPER_ACTION_CONTROLLER_IMPORT __attribute__( ( dllimport ) )
#else
#define GRIPPER_ACTION_CONTROLLER_EXPORT __declspec( dllexport )
#define GRIPPER_ACTION_CONTROLLER_IMPORT __declspec( dllimport )
#endif
#ifdef GRIPPER_ACTION_CONTROLLER_BUILDING_DLL
#define GRIPPER_ACTION_CONTROLLER_PUBLIC GRIPPER_ACTION_CONTROLLER_EXPORT
#else
#define GRIPPER_ACTION_CONTROLLER_PUBLIC GRIPPER_ACTION_CONTROLLER_IMPORT
#endif
#define GRIPPER_ACTION_CONTROLLER_PUBLIC_TYPE GRIPPER_ACTION_CONTROLLER_PUBLIC
#define GRIPPER_ACTION_CONTROLLER_LOCAL
#else
#define GRIPPER_ACTION_CONTROLLER_EXPORT __attribute__( ( visibility( "default" ) ) )
#define GRIPPER_ACTION_CONTROLLER_IMPORT
#if __GNUC__ >= 4
#define GRIPPER_ACTION_CONTROLLER_PUBLIC __attribute__( ( visibility( "default" ) ) )
#define GRIPPER_ACTION_CONTROLLER_LOCAL __attribute__( ( visibility( "hidden" ) ) )
#else
#define GRIPPER_ACTION_CONTROLLER_PUBLIC
#define GRIPPER_ACTION_CONTROLLER_LOCAL
#endif
#define GRIPPER_ACTION_CONTROLLER_PUBLIC_TYPE
#endif
#endif // GRIPPER_CONTROLLERS__VISIBILITY_CONTROL_HPP_
1 // Copyright 2014, SRI International
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/// \author Sachin Chitta
// Project
#include <gripper_controllers/gripper_action_controller.hpp>
#include <hardware_interface/types/hardware_interface_type_values.hpp>
namespace position_controllers
{
/**
* \brief Gripper action controller that sends
* commands to a \b position interface.
*/
using GripperActionController =
gripper_action_controller::GripperActionController<hardware_interface::HW_IF_POSITION>;
} // namespace position_controllers
namespace effort_controllers
{
/**
* \brief Gripper action controller that sends
* commands to a \b effort interface.
*/
using GripperActionController =
gripper_action_controller::GripperActionController<hardware_interface::HW_IF_EFFORT>;
} // namespace effort_controllers
#include "pluginlib/class_list_macros.hpp"
42 PLUGINLIB_EXPORT_CLASS(
position_controllers::GripperActionController, controller_interface::ControllerInterface )
PLUGINLIB_EXPORT_CLASS(
45 effort_controllers::GripperActionController, controller_interface::ControllerInterface )
1 // Copyright 2020 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gmock/gmock.h>
#include <memory>
#include "controller_manager/controller_manager.hpp"
#include "hardware_interface/resource_manager.hpp"
#include "rclcpp/executors/single_threaded_executor.hpp"
#include "ros2_control_test_assets/descriptions.hpp"
23 TEST( TestLoadGripperActionControllers, load_controller )
{
rclcpp::init( 0, nullptr );
std::shared_ptr<rclcpp::Executor> executor =
std::make_shared<rclcpp::executors::SingleThreadedExecutor>( );
controller_manager::ControllerManager cm(
std::make_unique<hardware_interface::ResourceManager>(
ros2_control_test_assets::minimal_robot_urdf ),
executor, "test_controller_manager" );
ASSERT_NO_THROW( cm.load_controller(
"test_gripper_action_position_controller", "position_controllers/GripperActionController" ) );
ASSERT_NO_THROW( cm.load_controller(
"test_gripper_action_effort_controller", "effort_controllers/GripperActionController" ) );
rclcpp::shutdown( );
}
1 // Copyright 2021 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*
* Authors: Subhas Das, Denis Stogl, Victor Lopez
*/
#ifndef IMU_SENSOR_BROADCASTER__IMU_SENSOR_BROADCASTER_HPP_
#define IMU_SENSOR_BROADCASTER__IMU_SENSOR_BROADCASTER_HPP_
#include <memory>
#include <string>
#include <vector>
#include "controller_interface/controller_interface.hpp"
#include "imu_sensor_broadcaster/visibility_control.h"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "realtime_tools/realtime_publisher.h"
#include "semantic_components/imu_sensor.hpp"
#include "sensor_msgs/msg/imu.hpp"
namespace imu_sensor_broadcaster
{
36 class IMUSensorBroadcaster : public controller_interface::ControllerInterface
{
public:
IMU_SENSOR_BROADCASTER_PUBLIC
controller_interface::InterfaceConfiguration command_interface_configuration( ) const override;
IMU_SENSOR_BROADCASTER_PUBLIC
controller_interface::InterfaceConfiguration state_interface_configuration( ) const override;
IMU_SENSOR_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init( ) override;
IMU_SENSOR_BROADCASTER_PUBLIC
controller_interface::CallbackReturn on_configure(
const rclcpp_lifecycle::State & previous_state ) override;
IMU_SENSOR_BROADCASTER_PUBLIC
controller_interface::CallbackReturn on_activate(
const rclcpp_lifecycle::State & previous_state ) override;
IMU_SENSOR_BROADCASTER_PUBLIC
controller_interface::CallbackReturn on_deactivate(
const rclcpp_lifecycle::State & previous_state ) override;
IMU_SENSOR_BROADCASTER_PUBLIC
controller_interface::return_type update(
const rclcpp::Time & time, const rclcpp::Duration & period ) override;
protected:
std::string sensor_name_;
std::string frame_id_;
std::unique_ptr<semantic_components::IMUSensor> imu_sensor_;
using StatePublisher = realtime_tools::RealtimePublisher<sensor_msgs::msg::Imu>;
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr sensor_state_publisher_;
std::unique_ptr<StatePublisher> realtime_publisher_;
};
} // namespace imu_sensor_broadcaster
#endif // IMU_SENSOR_BROADCASTER__IMU_SENSOR_BROADCASTER_HPP_
1 // Copyright ( c ) 2021, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*
* Author: Subhas Das, Denis Stogl
*/
#ifndef IMU_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_
#define IMU_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_
// This logic was borrowed ( then namespaced ) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define IMU_SENSOR_BROADCASTER_EXPORT __attribute__( ( dllexport ) )
#define IMU_SENSOR_BROADCASTER_IMPORT __attribute__( ( dllimport ) )
#else
#define IMU_SENSOR_BROADCASTER_EXPORT __declspec( dllexport )
#define IMU_SENSOR_BROADCASTER_IMPORT __declspec( dllimport )
#endif
#ifdef IMU_SENSOR_BROADCASTER_BUILDING_DLL
#define IMU_SENSOR_BROADCASTER_PUBLIC IMU_SENSOR_BROADCASTER_EXPORT
#else
#define IMU_SENSOR_BROADCASTER_PUBLIC IMU_SENSOR_BROADCASTER_IMPORT
#endif
#define IMU_SENSOR_BROADCASTER_PUBLIC_TYPE IMU_SENSOR_BROADCASTER_PUBLIC
#define IMU_SENSOR_BROADCASTER_LOCAL
#else
#define IMU_SENSOR_BROADCASTER_EXPORT __attribute__( ( visibility( "default" ) ) )
#define IMU_SENSOR_BROADCASTER_IMPORT
#if __GNUC__ >= 4
#define IMU_SENSOR_BROADCASTER_PUBLIC __attribute__( ( visibility( "default" ) ) )
#define IMU_SENSOR_BROADCASTER_LOCAL __attribute__( ( visibility( "hidden" ) ) )
#else
#define IMU_SENSOR_BROADCASTER_PUBLIC
#define IMU_SENSOR_BROADCASTER_LOCAL
#endif
#define IMU_SENSOR_BROADCASTER_PUBLIC_TYPE
#endif
#endif // IMU_SENSOR_BROADCASTER__VISIBILITY_CONTROL_H_
1 // Copyright 2021 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*
* Authors: Subhas Das, Denis Stogl, Victor Lopez
*/
#include "imu_sensor_broadcaster/imu_sensor_broadcaster.hpp"
#include <memory>
#include <string>
namespace imu_sensor_broadcaster
{
26 controller_interface::CallbackReturn IMUSensorBroadcaster::on_init( )
{
try
{
auto_declare<std::string>( "sensor_name", "" );
auto_declare<std::string>( "frame_id", "" );
}
catch ( const std::exception & e )
{
RCLCPP_ERROR(
get_node( )->get_logger( ), "Exception thrown during init stage with message: %s \n", e.what( ) );
return CallbackReturn::ERROR;
}
return CallbackReturn::SUCCESS;
}
43 controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure(
44 const rclcpp_lifecycle::State & /*previous_state*/ )
{
sensor_name_ = get_node( )->get_parameter( "sensor_name" ).as_string( );
if ( sensor_name_.empty( ) )
{
RCLCPP_ERROR( get_node( )->get_logger( ), "'sensor_name' parameter has to be specified." );
return CallbackReturn::ERROR;
}
frame_id_ = get_node( )->get_parameter( "frame_id" ).as_string( );
if ( frame_id_.empty( ) )
{
RCLCPP_ERROR( get_node( )->get_logger( ), "'frame_id' parameter has to be provided." );
return CallbackReturn::ERROR;
}
imu_sensor_ =
std::make_unique<semantic_components::IMUSensor>( semantic_components::IMUSensor( sensor_name_ ) );
try
{
// register ft sensor data publisher
sensor_state_publisher_ =
get_node( )->create_publisher<sensor_msgs::msg::Imu>( "~/imu", rclcpp::SystemDefaultsQoS( ) );
realtime_publisher_ = std::make_unique<StatePublisher>( sensor_state_publisher_ );
}
catch ( const std::exception & e )
{
fprintf(
stderr, "Exception thrown during publisher creation at configure stage with message : %s \n",
e.what( ) );
return CallbackReturn::ERROR;
}
realtime_publisher_->lock( );
realtime_publisher_->msg_.header.frame_id = frame_id_;
realtime_publisher_->unlock( );
RCLCPP_DEBUG( get_node( )->get_logger( ), "configure successful" );
return CallbackReturn::SUCCESS;
}
85 controller_interface::InterfaceConfiguration IMUSensorBroadcaster::command_interface_configuration( )
const
{
controller_interface::InterfaceConfiguration command_interfaces_config;
command_interfaces_config.type = controller_interface::interface_configuration_type::NONE;
return command_interfaces_config;
}
93 controller_interface::InterfaceConfiguration IMUSensorBroadcaster::state_interface_configuration( )
const
{
controller_interface::InterfaceConfiguration state_interfaces_config;
state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
state_interfaces_config.names = imu_sensor_->get_state_interface_names( );
return state_interfaces_config;
}
102 controller_interface::CallbackReturn IMUSensorBroadcaster::on_activate(
103 const rclcpp_lifecycle::State & /*previous_state*/ )
{
imu_sensor_->assign_loaned_state_interfaces( state_interfaces_ );
return CallbackReturn::SUCCESS;
}
109 controller_interface::CallbackReturn IMUSensorBroadcaster::on_deactivate(
110 const rclcpp_lifecycle::State & /*previous_state*/ )
{
imu_sensor_->release_interfaces( );
return CallbackReturn::SUCCESS;
}
116 controller_interface::return_type IMUSensorBroadcaster::update(
117 const rclcpp::Time & time, const rclcpp::Duration & /*period*/ )
{
if ( realtime_publisher_ && realtime_publisher_->trylock( ) )
{
realtime_publisher_->msg_.header.stamp = time;
imu_sensor_->get_values_as_message( realtime_publisher_->msg_ );
realtime_publisher_->unlockAndPublish( );
}
return controller_interface::return_type::OK;
}
} // namespace imu_sensor_broadcaster
#include "pluginlib/class_list_macros.hpp"
133 PLUGINLIB_EXPORT_CLASS(
imu_sensor_broadcaster::IMUSensorBroadcaster, controller_interface::ControllerInterface )
// Copyright 2021 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*
* Authors: Subhas Das, Denis Stogl, Victor Lopez
*/
#include "test_imu_sensor_broadcaster.hpp"
#include <memory>
#include <utility>
#include <vector>
#include "hardware_interface/loaned_state_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "imu_sensor_broadcaster/imu_sensor_broadcaster.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/utilities.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "sensor_msgs/msg/imu.hpp"
using hardware_interface::LoanedStateInterface;
namespace
{
38 constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS;
constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR;
} // namespace
void IMUSensorBroadcasterTest::SetUpTestCase( ) { rclcpp::init( 0, nullptr ); }
void IMUSensorBroadcasterTest::TearDownTestCase( ) { rclcpp::shutdown( ); }
void IMUSensorBroadcasterTest::SetUp( )
{
// initialize controller
imu_broadcaster_ = std::make_unique<FriendIMUSensorBroadcaster>( );
}
void IMUSensorBroadcasterTest::TearDown( ) { imu_broadcaster_.reset( nullptr ); }
void IMUSensorBroadcasterTest::SetUpIMUBroadcaster( )
{
const auto result = imu_broadcaster_->init( "test_imu_sensor_broadcaster" );
ASSERT_EQ( result, controller_interface::return_type::OK );
std::vector<LoanedStateInterface> state_ifs;
state_ifs.emplace_back( imu_orientation_x_ );
state_ifs.emplace_back( imu_orientation_y_ );
state_ifs.emplace_back( imu_orientation_z_ );
state_ifs.emplace_back( imu_orientation_w_ );
state_ifs.emplace_back( imu_angular_velocity_x_ );
state_ifs.emplace_back( imu_angular_velocity_y_ );
state_ifs.emplace_back( imu_angular_velocity_z_ );
state_ifs.emplace_back( imu_linear_acceleration_x_ );
state_ifs.emplace_back( imu_linear_acceleration_y_ );
state_ifs.emplace_back( imu_linear_acceleration_z_ );
imu_broadcaster_->assign_interfaces( {}, std::move( state_ifs ) );
}
void IMUSensorBroadcasterTest::subscribe_and_get_message( sensor_msgs::msg::Imu & imu_msg )
{
// create a new subscriber
rclcpp::Node test_subscription_node( "test_subscription_node" );
auto subs_callback = [&]( const sensor_msgs::msg::Imu::SharedPtr ) {};
auto subscription = test_subscription_node.create_subscription<sensor_msgs::msg::Imu>(
"/test_imu_sensor_broadcaster/imu", 10, subs_callback );
// call update to publish the test value
// since update doesn't guarantee a published message, republish until received
int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop
rclcpp::WaitSet wait_set; // block used to wait on message
wait_set.add_subscription( subscription );
while ( max_sub_check_loop_count-- )
{
imu_broadcaster_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
// check if message has been received
if ( wait_set.wait( std::chrono::milliseconds( 2 ) ).kind( ) == rclcpp::WaitResultKind::Ready )
{
break;
}
}
ASSERT_GE( max_sub_check_loop_count, 0 ) << "Test was unable to publish a message through "
"controller/broadcaster update loop";
// take message from subscription
rclcpp::MessageInfo msg_info;
ASSERT_TRUE( subscription->take( imu_msg, msg_info ) );
}
TEST_F( IMUSensorBroadcasterTest, SensorName_InterfaceNames_NotSet )
{
SetUpIMUBroadcaster( );
// configure failed
ASSERT_EQ( imu_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_ERROR );
}
TEST_F( IMUSensorBroadcasterTest, SensorName_FrameId_NotSet )
{
SetUpIMUBroadcaster( );
// set the 'interface_names'
imu_broadcaster_->get_node( )->set_parameter(
{"interface_names.angular_velocity.x", "imu_sensor/angular_velocity.x"} );
imu_broadcaster_->get_node( )->set_parameter(
{"interface_names.linear_acceleration.z", "imu_sensor/linear_acceleration.z"} );
// configure failed, 'frame_id' parameter not set
ASSERT_EQ( imu_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_ERROR );
}
TEST_F( IMUSensorBroadcasterTest, InterfaceNames_FrameId_NotSet )
{
SetUpIMUBroadcaster( );
// set the 'sensor_name'
imu_broadcaster_->get_node( )->set_parameter( {"sensor_name", sensor_name_} );
// configure failed, 'frame_id' parameter not set
ASSERT_EQ( imu_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_ERROR );
}
TEST_F( IMUSensorBroadcasterTest, SensorName_Configure_Success )
{
SetUpIMUBroadcaster( );
// set the 'sensor_name'
imu_broadcaster_->get_node( )->set_parameter( {"sensor_name", sensor_name_} );
// set the 'frame_id'
imu_broadcaster_->get_node( )->set_parameter( {"frame_id", frame_id_} );
// configure passed
ASSERT_EQ( imu_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
}
TEST_F( IMUSensorBroadcasterTest, SensorName_Activate_Success )
{
SetUpIMUBroadcaster( );
// set the params 'sensor_name' and 'frame_id'
imu_broadcaster_->get_node( )->set_parameter( {"sensor_name", sensor_name_} );
imu_broadcaster_->get_node( )->set_parameter( {"frame_id", frame_id_} );
// configure and activate success
ASSERT_EQ( imu_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
ASSERT_EQ( imu_broadcaster_->on_activate( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
}
TEST_F( IMUSensorBroadcasterTest, SensorName_Update_Success )
{
SetUpIMUBroadcaster( );
// set the params 'sensor_name' and 'frame_id'
imu_broadcaster_->get_node( )->set_parameter( {"sensor_name", sensor_name_} );
imu_broadcaster_->get_node( )->set_parameter( {"frame_id", frame_id_} );
ASSERT_EQ( imu_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
ASSERT_EQ( imu_broadcaster_->on_activate( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
ASSERT_EQ(
imu_broadcaster_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
}
TEST_F( IMUSensorBroadcasterTest, SensorName_Publish_Success )
{
SetUpIMUBroadcaster( );
// set the params 'sensor_name' and 'frame_id'
imu_broadcaster_->get_node( )->set_parameter( {"sensor_name", sensor_name_} );
imu_broadcaster_->get_node( )->set_parameter( {"frame_id", frame_id_} );
ASSERT_EQ( imu_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
ASSERT_EQ( imu_broadcaster_->on_activate( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
sensor_msgs::msg::Imu imu_msg;
subscribe_and_get_message( imu_msg );
ASSERT_EQ( imu_msg.header.frame_id, frame_id_ );
ASSERT_EQ( imu_msg.orientation.x, sensor_values_[0] );
ASSERT_EQ( imu_msg.orientation.y, sensor_values_[1] );
ASSERT_EQ( imu_msg.orientation.z, sensor_values_[2] );
ASSERT_EQ( imu_msg.orientation.w, sensor_values_[3] );
ASSERT_EQ( imu_msg.angular_velocity.x, sensor_values_[4] );
ASSERT_EQ( imu_msg.angular_velocity.y, sensor_values_[5] );
ASSERT_EQ( imu_msg.angular_velocity.z, sensor_values_[6] );
ASSERT_EQ( imu_msg.linear_acceleration.x, sensor_values_[7] );
ASSERT_EQ( imu_msg.linear_acceleration.y, sensor_values_[8] );
ASSERT_EQ( imu_msg.linear_acceleration.z, sensor_values_[9] );
}
// Copyright 2021 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*
* Authors: Subhas Das, Denis Stogl, Victor Lopez
*/
#ifndef TEST_IMU_SENSOR_BROADCASTER_HPP_
#define TEST_IMU_SENSOR_BROADCASTER_HPP_
#include <memory>
#include <string>
#include "gmock/gmock.h"
#include "imu_sensor_broadcaster/imu_sensor_broadcaster.hpp"
// subclassing and friending so we can access member variables
30 class FriendIMUSensorBroadcaster : public imu_sensor_broadcaster::IMUSensorBroadcaster
{
32 FRIEND_TEST( IMUSensorBroadcasterTest, SensorNameParameterNotSet );
33 FRIEND_TEST( IMUSensorBroadcasterTest, InterfaceNamesParameterNotSet );
34 FRIEND_TEST( IMUSensorBroadcasterTest, FrameIdParameterNotSet );
35 FRIEND_TEST( IMUSensorBroadcasterTest, SensorNameParameterIsEmpty );
36 FRIEND_TEST( IMUSensorBroadcasterTest, InterfaceNameParameterIsEmpty );
38 FRIEND_TEST( IMUSensorBroadcasterTest, ActivateSuccess );
39 FRIEND_TEST( IMUSensorBroadcasterTest, UpdateTest );
40 FRIEND_TEST( IMUSensorBroadcasterTest, SensorStatePublishTest );
};
43 class IMUSensorBroadcasterTest : public ::testing::Test
{
public:
46 static void SetUpTestCase( );
47 static void TearDownTestCase( );
49 void SetUp( );
50 void TearDown( );
52 void SetUpIMUBroadcaster( );
protected:
55 const std::string sensor_name_ = "imu_sensor";
56 const std::string frame_id_ = "imu_sensor_frame";
57 std::array<double, 10> sensor_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6, 7.7, 8.8, 9.9, 10.10};
hardware_interface::StateInterface imu_orientation_x_{
sensor_name_, "orientation.x", &sensor_values_[0]};
hardware_interface::StateInterface imu_orientation_y_{
61 sensor_name_, "orientation.y", &sensor_values_[1]};
hardware_interface::StateInterface imu_orientation_z_{
sensor_name_, "orientation.z", &sensor_values_[2]};
hardware_interface::StateInterface imu_orientation_w_{
sensor_name_, "orientation.w", &sensor_values_[3]};
hardware_interface::StateInterface imu_angular_velocity_x_{
sensor_name_, "angular_velocity.x", &sensor_values_[4]};
hardware_interface::StateInterface imu_angular_velocity_y_{
sensor_name_, "angular_velocity.y", &sensor_values_[5]};
hardware_interface::StateInterface imu_angular_velocity_z_{
sensor_name_, "angular_velocity.z", &sensor_values_[6]};
hardware_interface::StateInterface imu_linear_acceleration_x_{
sensor_name_, "linear_acceleration.x", &sensor_values_[7]};
hardware_interface::StateInterface imu_linear_acceleration_y_{
sensor_name_, "linear_acceleration.y", &sensor_values_[8]};
hardware_interface::StateInterface imu_linear_acceleration_z_{
sensor_name_, "linear_acceleration.z", &sensor_values_[9]};
std::unique_ptr<FriendIMUSensorBroadcaster> imu_broadcaster_;
void subscribe_and_get_message( sensor_msgs::msg::Imu & imu_msg );
};
#endif // TEST_IMU_SENSOR_BROADCASTER_HPP_
1 // Copyright ( c ) 2021, Stogl Robotics Consulting UG ( haftungsbeschränkt )
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*
* Author: Subhas Das, Denis Stogl
*/
#include <gmock/gmock.h>
#include <memory>
#include "controller_manager/controller_manager.hpp"
#include "hardware_interface/resource_manager.hpp"
#include "rclcpp/executor.hpp"
#include "rclcpp/executors/single_threaded_executor.hpp"
#include "rclcpp/utilities.hpp"
#include "ros2_control_test_assets/descriptions.hpp"
29 TEST( TestLoadIMUSensorBroadcaster, load_controller )
{
rclcpp::init( 0, nullptr );
std::shared_ptr<rclcpp::Executor> executor =
std::make_shared<rclcpp::executors::SingleThreadedExecutor>( );
controller_manager::ControllerManager cm(
std::make_unique<hardware_interface::ResourceManager>(
ros2_control_test_assets::minimal_robot_urdf ),
executor, "test_controller_manager" );
ASSERT_NO_THROW( cm.load_controller(
"test_imu_sensor_broadcaster", "imu_sensor_broadcaster/IMUSensorBroadcaster" ) );
rclcpp::shutdown( );
}
1 // Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef JOINT_STATE_BROADCASTER__JOINT_STATE_BROADCASTER_HPP_
#define JOINT_STATE_BROADCASTER__JOINT_STATE_BROADCASTER_HPP_
#include <memory>
#include <string>
#include <unordered_map>
#include <vector>
#include "control_msgs/msg/dynamic_joint_state.hpp"
#include "controller_interface/controller_interface.hpp"
#include "joint_state_broadcaster/visibility_control.h"
#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "realtime_tools/realtime_publisher.h"
#include "sensor_msgs/msg/joint_state.hpp"
namespace joint_state_broadcaster
{
/**
* \brief Joint State Broadcaster for all or some state in a ros2_control system.
*
* JointStateBroadcaster publishes state interfaces from ros2_control as ROS messages.
* There is a possibility to publish all available states ( typical use ), or only specific ones.
* The latter is, for example, used when hardware provides multiple measurement sources for some
* of its states, e.g., position.
* It is possible to define a mapping of measurements
* from different sources stored in custom interfaces to standard dynamic names in JointState
* message.
* If "joints" or "interfaces" parameter is empty, all available states are published.
*
* \param use_local_topics Flag to publish topics in local namespace.
* \param joints Names of the joints to publish.
* \param interfaces Names of interfaces to publish.
* \param map_interface_to_joint_state.{HW_IF_POSITION|HW_IF_VELOCITY|HW_IF_EFFORT} mapping
* between custom interface names and standard names in sensor_msgs::msg::JointState message.
*
* Publishes to:
* - \b joint_states ( sensor_msgs::msg::JointState ): Joint states related to movement
* ( position, velocity, effort ).
* - \b dynamic_joint_states ( control_msgs::msg::DynamicJointState ): Joint states regardless of
* its interface type.
*/
57 class JointStateBroadcaster : public controller_interface::ControllerInterface
{
public:
JOINT_STATE_BROADCASTER_PUBLIC
61 JointStateBroadcaster( );
JOINT_STATE_BROADCASTER_PUBLIC
controller_interface::InterfaceConfiguration command_interface_configuration( ) const override;
JOINT_STATE_BROADCASTER_PUBLIC
controller_interface::InterfaceConfiguration state_interface_configuration( ) const override;
JOINT_STATE_BROADCASTER_PUBLIC
controller_interface::return_type update(
const rclcpp::Time & time, const rclcpp::Duration & period ) override;
JOINT_STATE_BROADCASTER_PUBLIC
controller_interface::CallbackReturn on_init( ) override;
JOINT_STATE_BROADCASTER_PUBLIC
controller_interface::CallbackReturn on_configure(
const rclcpp_lifecycle::State & previous_state ) override;
JOINT_STATE_BROADCASTER_PUBLIC
controller_interface::CallbackReturn on_activate(
const rclcpp_lifecycle::State & previous_state ) override;
JOINT_STATE_BROADCASTER_PUBLIC
controller_interface::CallbackReturn on_deactivate(
const rclcpp_lifecycle::State & previous_state ) override;
protected:
bool init_joint_data( );
void init_joint_state_msg( );
void init_dynamic_joint_state_msg( );
bool use_all_available_interfaces( ) const;
protected:
// Optional parameters
bool use_local_topics_;
std::vector<std::string> joints_;
std::vector<std::string> interfaces_;
std::unordered_map<std::string, std::string> map_interface_to_joint_state_;
// For the JointState message,
// we store the name of joints with compatible interfaces
std::vector<std::string> joint_names_;
std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::JointState>> joint_state_publisher_;
std::shared_ptr<realtime_tools::RealtimePublisher<sensor_msgs::msg::JointState>>
realtime_joint_state_publisher_;
// For the DynamicJointState format, we use a map to buffer values in for easier lookup
// This allows to preserve whatever order or names/interfaces were initialized.
std::unordered_map<std::string, std::unordered_map<std::string, double>> name_if_value_mapping_;
std::shared_ptr<rclcpp::Publisher<control_msgs::msg::DynamicJointState>>
dynamic_joint_state_publisher_;
std::shared_ptr<realtime_tools::RealtimePublisher<control_msgs::msg::DynamicJointState>>
realtime_dynamic_joint_state_publisher_;
};
} // namespace joint_state_broadcaster
#endif // JOINT_STATE_BROADCASTER__JOINT_STATE_BROADCASTER_HPP_
1 // Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/* This header must be included by all rclcpp headers which declare symbols
* which are defined in the rclcpp library. When not building the rclcpp
* library, i.e. when using the headers in other package's code, the contents
* of this header change the visibility of certain symbols which the rclcpp
* library cannot have, but the consuming code must have inorder to link.
*/
#ifndef JOINT_STATE_BROADCASTER__VISIBILITY_CONTROL_H_
#define JOINT_STATE_BROADCASTER__VISIBILITY_CONTROL_H_
// This logic was borrowed ( then namespaced ) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define JOINT_STATE_BROADCASTER_EXPORT __attribute__( ( dllexport ) )
#define JOINT_STATE_BROADCASTER_IMPORT __attribute__( ( dllimport ) )
#else
#define JOINT_STATE_BROADCASTER_EXPORT __declspec( dllexport )
#define JOINT_STATE_BROADCASTER_IMPORT __declspec( dllimport )
#endif
#ifdef JOINT_STATE_BROADCASTER_BUILDING_DLL
#define JOINT_STATE_BROADCASTER_PUBLIC JOINT_STATE_BROADCASTER_EXPORT
#else
#define JOINT_STATE_BROADCASTER_PUBLIC JOINT_STATE_BROADCASTER_IMPORT
#endif
#define JOINT_STATE_BROADCASTER_PUBLIC_TYPE JOINT_STATE_BROADCASTER_PUBLIC
#define JOINT_STATE_BROADCASTER_LOCAL
#else
#define JOINT_STATE_BROADCASTER_EXPORT __attribute__( ( visibility( "default" ) ) )
#define JOINT_STATE_BROADCASTER_IMPORT
#if __GNUC__ >= 4
#define JOINT_STATE_BROADCASTER_PUBLIC __attribute__( ( visibility( "default" ) ) )
#define JOINT_STATE_BROADCASTER_LOCAL __attribute__( ( visibility( "hidden" ) ) )
#else
#define JOINT_STATE_BROADCASTER_PUBLIC
#define JOINT_STATE_BROADCASTER_LOCAL
#endif
#define JOINT_STATE_BROADCASTER_PUBLIC_TYPE
#endif
#endif // JOINT_STATE_BROADCASTER__VISIBILITY_CONTROL_H_
// Copyright 2021 ros2_control development team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "joint_state_broadcaster/joint_state_broadcaster.hpp"
#include <stddef.h>
#include <limits>
#include <memory>
#include <string>
#include <unordered_map>
#include <vector>
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/clock.hpp"
#include "rclcpp/qos.hpp"
#include "rclcpp/qos_event.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "rcpputils/split.hpp"
#include "rcutils/logging_macros.h"
#include "std_msgs/msg/header.hpp"
namespace rclcpp_lifecycle
{
37 class State;
} // namespace rclcpp_lifecycle
namespace joint_state_broadcaster
{
42 const auto kUninitializedValue = std::numeric_limits<double>::quiet_NaN( );
using hardware_interface::HW_IF_EFFORT;
using hardware_interface::HW_IF_POSITION;
using hardware_interface::HW_IF_VELOCITY;
47 JointStateBroadcaster::JointStateBroadcaster( ) {}
49 controller_interface::CallbackReturn JointStateBroadcaster::on_init( )
{
try
{
auto_declare<bool>( "use_local_topics", false );
auto_declare<std::vector<std::string>>( "joints", std::vector<std::string>( {} ) );
auto_declare<std::vector<std::string>>( "interfaces", std::vector<std::string>( {} ) );
auto_declare<std::string>(
std::string( "map_interface_to_joint_state." ) + HW_IF_POSITION, HW_IF_POSITION );
auto_declare<std::string>(
std::string( "map_interface_to_joint_state." ) + HW_IF_VELOCITY, HW_IF_VELOCITY );
auto_declare<std::string>(
std::string( "map_interface_to_joint_state." ) + HW_IF_EFFORT, HW_IF_EFFORT );
}
catch ( const std::exception & e )
{
fprintf( stderr, "Exception thrown during init stage with message: %s \n", e.what( ) );
return CallbackReturn::ERROR;
}
return CallbackReturn::SUCCESS;
}
controller_interface::InterfaceConfiguration
73 JointStateBroadcaster::command_interface_configuration( ) const
{
return controller_interface::InterfaceConfiguration{
controller_interface::interface_configuration_type::NONE};
}
79 controller_interface::InterfaceConfiguration JointStateBroadcaster::state_interface_configuration( )
const
{
controller_interface::InterfaceConfiguration state_interfaces_config;
if ( use_all_available_interfaces( ) )
{
state_interfaces_config.type = controller_interface::interface_configuration_type::ALL;
}
else
{
state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
for ( const auto & joint : joints_ )
{
for ( const auto & interface : interfaces_ )
{
state_interfaces_config.names.push_back( joint + "/" + interface );
}
}
}
return state_interfaces_config;
}
103 controller_interface::CallbackReturn JointStateBroadcaster::on_configure(
104 const rclcpp_lifecycle::State & /*previous_state*/ )
{
use_local_topics_ = get_node( )->get_parameter( "use_local_topics" ).as_bool( );
joints_ = get_node( )->get_parameter( "joints" ).as_string_array( );
interfaces_ = get_node( )->get_parameter( "interfaces" ).as_string_array( );
if ( use_all_available_interfaces( ) )
{
RCLCPP_INFO(
get_node( )->get_logger( ),
"'joints' or 'interfaces' parameter is empty. "
"All available state interfaces will be published" );
joints_.clear( );
interfaces_.clear( );
}
else
{
RCLCPP_INFO(
get_node( )->get_logger( ),
"Publishing state interfaces defined in 'joints' and 'interfaces' parameters." );
}
auto get_map_interface_parameter = [&]( const std::string & interface ) {
std::string interface_to_map =
get_node( )
->get_parameter( std::string( "map_interface_to_joint_state." ) + interface )
.as_string( );
if ( std::find( interfaces_.begin( ), interfaces_.end( ), interface ) != interfaces_.end( ) )
{
map_interface_to_joint_state_[interface] = interface;
RCLCPP_WARN(
get_node( )->get_logger( ),
"Mapping from '%s' to interface '%s' will not be done, because '%s' is defined "
"in 'interface' parameter.",
interface_to_map.c_str( ), interface.c_str( ), interface.c_str( ) );
}
else
{
map_interface_to_joint_state_[interface_to_map] = interface;
}
};
map_interface_to_joint_state_ = {};
get_map_interface_parameter( HW_IF_POSITION );
get_map_interface_parameter( HW_IF_VELOCITY );
get_map_interface_parameter( HW_IF_EFFORT );
try
{
const std::string topic_name_prefix = use_local_topics_ ? "~/" : "";
joint_state_publisher_ = get_node( )->create_publisher<sensor_msgs::msg::JointState>(
topic_name_prefix + "joint_states", rclcpp::SystemDefaultsQoS( ) );
realtime_joint_state_publisher_ =
std::make_shared<realtime_tools::RealtimePublisher<sensor_msgs::msg::JointState>>(
joint_state_publisher_ );
dynamic_joint_state_publisher_ =
get_node( )->create_publisher<control_msgs::msg::DynamicJointState>(
topic_name_prefix + "dynamic_joint_states", rclcpp::SystemDefaultsQoS( ) );
realtime_dynamic_joint_state_publisher_ =
std::make_shared<realtime_tools::RealtimePublisher<control_msgs::msg::DynamicJointState>>(
dynamic_joint_state_publisher_ );
}
catch ( const std::exception & e )
{
// get_node( ) may throw, logging raw here
fprintf( stderr, "Exception thrown during init stage with message: %s \n", e.what( ) );
return CallbackReturn::ERROR;
}
return CallbackReturn::SUCCESS;
}
180 controller_interface::CallbackReturn JointStateBroadcaster::on_activate(
181 const rclcpp_lifecycle::State & /*previous_state*/ )
{
if ( !init_joint_data( ) )
{
RCLCPP_ERROR(
get_node( )->get_logger( ), "None of requested interfaces exist. Controller will not run." );
return CallbackReturn::ERROR;
}
init_joint_state_msg( );
init_dynamic_joint_state_msg( );
if (
!use_all_available_interfaces( ) &&
state_interfaces_.size( ) != ( joints_.size( ) * interfaces_.size( ) ) )
{
RCLCPP_WARN(
get_node( )->get_logger( ),
"Not all requested interfaces exists. "
"Check ControllerManager output for more detailed information." );
}
return CallbackReturn::SUCCESS;
}
206 controller_interface::CallbackReturn JointStateBroadcaster::on_deactivate(
207 const rclcpp_lifecycle::State & /*previous_state*/ )
{
return CallbackReturn::SUCCESS;
}
template <typename T>
213 bool has_any_key(
const std::unordered_map<std::string, T> & map, const std::vector<std::string> & keys )
{
bool found_key = false;
for ( const auto & key_item : map )
{
const auto & key = key_item.first;
if ( std::find( keys.cbegin( ), keys.cend( ), key ) != keys.cend( ) )
{
found_key = true;
break;
}
}
return found_key;
}
229 bool JointStateBroadcaster::init_joint_data( )
{
if ( state_interfaces_.empty( ) )
{
return false;
}
// loop in reverse order, this maintains the order of values at retrieval time
for ( auto si = state_interfaces_.crbegin( ); si != state_interfaces_.crend( ); si++ )
{
// initialize map if name is new
if ( name_if_value_mapping_.count( si->get_prefix_name( ) ) == 0 )
{
name_if_value_mapping_[si->get_prefix_name( )] = {};
}
// add interface name
std::string interface_name = si->get_interface_name( );
if ( map_interface_to_joint_state_.count( interface_name ) > 0 )
{
interface_name = map_interface_to_joint_state_[interface_name];
}
name_if_value_mapping_[si->get_prefix_name( )][interface_name] = kUninitializedValue;
}
// filter state interfaces that have at least one of the joint_states fields,
// the rest will be ignored for this message
for ( const auto & name_ifv : name_if_value_mapping_ )
{
const auto & interfaces_and_values = name_ifv.second;
if ( has_any_key( interfaces_and_values, {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT} ) )
{
joint_names_.push_back( name_ifv.first );
}
}
// Add extra joints from parameters, each joint will be added to joint_names_ and
// name_if_value_mapping_ if it is not already there
rclcpp::Parameter extra_joints;
if ( get_node( )->get_parameter( "extra_joints", extra_joints ) )
{
const std::vector<std::string> & extra_joints_names = extra_joints.as_string_array( );
for ( const auto & extra_joint_name : extra_joints_names )
{
if ( name_if_value_mapping_.count( extra_joint_name ) == 0 )
{
name_if_value_mapping_[extra_joint_name] = {
{HW_IF_POSITION, 0.0}, {HW_IF_VELOCITY, 0.0}, {HW_IF_EFFORT, 0.0}};
joint_names_.push_back( extra_joint_name );
}
}
}
return true;
}
284 void JointStateBroadcaster::init_joint_state_msg( )
{
const size_t num_joints = joint_names_.size( );
/// @note joint_state_msg publishes position, velocity and effort for all joints,
/// with at least one of these interfaces, the rest are omitted from this message
// default initialization for joint state message
auto & joint_state_msg = realtime_joint_state_publisher_->msg_;
joint_state_msg.name = joint_names_;
joint_state_msg.position.resize( num_joints, kUninitializedValue );
joint_state_msg.velocity.resize( num_joints, kUninitializedValue );
joint_state_msg.effort.resize( num_joints, kUninitializedValue );
}
299 void JointStateBroadcaster::init_dynamic_joint_state_msg( )
{
auto & dynamic_joint_state_msg = realtime_dynamic_joint_state_publisher_->msg_;
for ( const auto & name_ifv : name_if_value_mapping_ )
{
const auto & name = name_ifv.first;
const auto & interfaces_and_values = name_ifv.second;
dynamic_joint_state_msg.joint_names.push_back( name );
control_msgs::msg::InterfaceValue if_value;
for ( const auto & interface_and_value : interfaces_and_values )
{
if_value.interface_names.emplace_back( interface_and_value.first );
if_value.values.emplace_back( kUninitializedValue );
}
dynamic_joint_state_msg.interface_values.emplace_back( if_value );
}
}
317 bool JointStateBroadcaster::use_all_available_interfaces( ) const
{
return joints_.empty( ) || interfaces_.empty( );
}
322 double get_value(
const std::unordered_map<std::string, std::unordered_map<std::string, double>> & map,
const std::string & name, const std::string & interface_name )
{
const auto & interfaces_and_values = map.at( name );
const auto interface_and_value = interfaces_and_values.find( interface_name );
if ( interface_and_value != interfaces_and_values.cend( ) )
{
return interface_and_value->second;
}
else
{
return kUninitializedValue;
}
}
338 controller_interface::return_type JointStateBroadcaster::update(
339 const rclcpp::Time & time, const rclcpp::Duration & /*period*/ )
{
for ( const auto & state_interface : state_interfaces_ )
{
std::string interface_name = state_interface.get_interface_name( );
if ( map_interface_to_joint_state_.count( interface_name ) > 0 )
{
interface_name = map_interface_to_joint_state_[interface_name];
}
name_if_value_mapping_[state_interface.get_prefix_name( )][interface_name] =
state_interface.get_value( );
RCLCPP_DEBUG(
get_node( )->get_logger( ), "%s: %f\n", state_interface.get_name( ).c_str( ),
state_interface.get_value( ) );
}
if ( realtime_joint_state_publisher_ && realtime_joint_state_publisher_->trylock( ) )
{
auto & joint_state_msg = realtime_joint_state_publisher_->msg_;
joint_state_msg.header.stamp = time;
// update joint state message and dynamic joint state message
for ( size_t i = 0; i < joint_names_.size( ); ++i )
{
joint_state_msg.position[i] =
get_value( name_if_value_mapping_, joint_names_[i], HW_IF_POSITION );
joint_state_msg.velocity[i] =
get_value( name_if_value_mapping_, joint_names_[i], HW_IF_VELOCITY );
joint_state_msg.effort[i] = get_value( name_if_value_mapping_, joint_names_[i], HW_IF_EFFORT );
}
realtime_joint_state_publisher_->unlockAndPublish( );
}
if ( realtime_dynamic_joint_state_publisher_ && realtime_dynamic_joint_state_publisher_->trylock( ) )
{
auto & dynamic_joint_state_msg = realtime_dynamic_joint_state_publisher_->msg_;
dynamic_joint_state_msg.header.stamp = time;
for ( size_t joint_index = 0; joint_index < dynamic_joint_state_msg.joint_names.size( );
++joint_index )
{
const auto & name = dynamic_joint_state_msg.joint_names[joint_index];
for ( size_t interface_index = 0;
interface_index <
dynamic_joint_state_msg.interface_values[joint_index].interface_names.size( );
++interface_index )
{
const auto & interface_name =
dynamic_joint_state_msg.interface_values[joint_index].interface_names[interface_index];
dynamic_joint_state_msg.interface_values[joint_index].values[interface_index] =
name_if_value_mapping_[name][interface_name];
}
}
realtime_dynamic_joint_state_publisher_->unlockAndPublish( );
}
return controller_interface::return_type::OK;
}
} // namespace joint_state_broadcaster
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(
joint_state_broadcaster::JointStateBroadcaster, controller_interface::ControllerInterface )
// Copyright 2020 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <stddef.h>
#include <functional>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "gmock/gmock.h"
#include "hardware_interface/loaned_state_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "joint_state_broadcaster/joint_state_broadcaster.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/utilities.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "test_joint_state_broadcaster.hpp"
using hardware_interface::HW_IF_EFFORT;
using hardware_interface::HW_IF_POSITION;
using hardware_interface::HW_IF_VELOCITY;
using hardware_interface::LoanedStateInterface;
using std::placeholders::_1;
using testing::Each;
using testing::ElementsAreArray;
using testing::IsEmpty;
using testing::SizeIs;
namespace
{
46 constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS;
constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR;
} // namespace
void JointStateBroadcasterTest::SetUpTestCase( ) { rclcpp::init( 0, nullptr ); }
void JointStateBroadcasterTest::TearDownTestCase( ) { rclcpp::shutdown( ); }
void JointStateBroadcasterTest::SetUp( )
{
// initialize broadcaster
state_broadcaster_ = std::make_unique<FriendJointStateBroadcaster>( );
}
void JointStateBroadcasterTest::TearDown( ) { state_broadcaster_.reset( nullptr ); }
void JointStateBroadcasterTest::SetUpStateBroadcaster(
const std::vector<std::string> & joint_names, const std::vector<std::string> & interfaces )
{
init_broadcaster_and_set_parameters( joint_names, interfaces );
assign_state_interfaces( joint_names, interfaces );
}
void JointStateBroadcasterTest::init_broadcaster_and_set_parameters(
const std::vector<std::string> & joint_names, const std::vector<std::string> & interfaces )
{
const auto result = state_broadcaster_->init( "joint_state_broadcaster" );
ASSERT_EQ( result, controller_interface::return_type::OK );
state_broadcaster_->get_node( )->set_parameter( {"joints", joint_names} );
state_broadcaster_->get_node( )->set_parameter( {"interfaces", interfaces} );
}
void JointStateBroadcasterTest::assign_state_interfaces(
const std::vector<std::string> & joint_names, const std::vector<std::string> & interfaces )
{
std::vector<LoanedStateInterface> state_ifs;
if ( joint_names.empty( ) || interfaces.empty( ) )
{
state_ifs.emplace_back( joint_1_pos_state_ );
state_ifs.emplace_back( joint_2_pos_state_ );
state_ifs.emplace_back( joint_3_pos_state_ );
state_ifs.emplace_back( joint_1_vel_state_ );
state_ifs.emplace_back( joint_2_vel_state_ );
state_ifs.emplace_back( joint_3_vel_state_ );
state_ifs.emplace_back( joint_1_eff_state_ );
state_ifs.emplace_back( joint_2_eff_state_ );
state_ifs.emplace_back( joint_3_eff_state_ );
}
else
{
for ( const auto & joint : joint_names )
{
for ( const auto & interface : interfaces )
{
if ( joint == joint_names_[0] && interface == interface_names_[0] )
{
state_ifs.emplace_back( joint_1_pos_state_ );
}
if ( joint == joint_names_[1] && interface == interface_names_[0] )
{
state_ifs.emplace_back( joint_2_pos_state_ );
}
if ( joint == joint_names_[2] && interface == interface_names_[0] )
{
state_ifs.emplace_back( joint_3_pos_state_ );
}
if ( joint == joint_names_[0] && interface == interface_names_[1] )
{
state_ifs.emplace_back( joint_1_vel_state_ );
}
if ( joint == joint_names_[1] && interface == interface_names_[1] )
{
state_ifs.emplace_back( joint_2_vel_state_ );
}
if ( joint == joint_names_[2] && interface == interface_names_[1] )
{
state_ifs.emplace_back( joint_3_vel_state_ );
}
if ( joint == joint_names_[0] && interface == interface_names_[2] )
{
state_ifs.emplace_back( joint_1_eff_state_ );
}
if ( joint == joint_names_[1] && interface == interface_names_[2] )
{
state_ifs.emplace_back( joint_2_eff_state_ );
}
if ( joint == joint_names_[2] && interface == interface_names_[2] )
{
state_ifs.emplace_back( joint_3_eff_state_ );
}
if ( interface == custom_interface_name_ )
{
state_ifs.emplace_back( joint_X_custom_state );
}
}
}
}
state_broadcaster_->assign_interfaces( {}, std::move( state_ifs ) );
}
TEST_F( JointStateBroadcasterTest, ConfigureErrorTest )
{
// publishers not initialized yet
ASSERT_FALSE( state_broadcaster_->realtime_joint_state_publisher_ );
ASSERT_FALSE( state_broadcaster_->realtime_dynamic_joint_state_publisher_ );
// configure failed
ASSERT_THROW( state_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), std::exception );
SetUpStateBroadcaster( );
// check state remains unchanged
// publishers still not initialized
ASSERT_FALSE( state_broadcaster_->realtime_joint_state_publisher_ );
ASSERT_FALSE( state_broadcaster_->realtime_dynamic_joint_state_publisher_ );
}
TEST_F( JointStateBroadcasterTest, ActivateTest )
{
// publishers not initialized yet
ASSERT_FALSE( state_broadcaster_->joint_state_publisher_ );
ASSERT_FALSE( state_broadcaster_->dynamic_joint_state_publisher_ );
SetUpStateBroadcaster( );
// configure ok
ASSERT_EQ( state_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
ASSERT_EQ( state_broadcaster_->on_activate( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
const size_t NUM_JOINTS = joint_names_.size( );
// publishers initialized
ASSERT_TRUE( state_broadcaster_->realtime_joint_state_publisher_ );
ASSERT_TRUE( state_broadcaster_->realtime_dynamic_joint_state_publisher_ );
// joint state initialized
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
ASSERT_THAT( joint_state_msg.name, ElementsAreArray( joint_names_ ) );
ASSERT_THAT( joint_state_msg.position, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( joint_state_msg.velocity, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( joint_state_msg.effort, SizeIs( NUM_JOINTS ) );
// dynamic joint state initialized
const auto & dynamic_joint_state_msg =
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
ASSERT_THAT( dynamic_joint_state_msg.joint_names, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( dynamic_joint_state_msg.interface_values, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( dynamic_joint_state_msg.joint_names, ElementsAreArray( joint_names_ ) );
ASSERT_THAT(
dynamic_joint_state_msg.interface_values[0].interface_names,
ElementsAreArray( interface_names_ ) );
ASSERT_THAT(
dynamic_joint_state_msg.interface_values[1].interface_names,
ElementsAreArray( interface_names_ ) );
ASSERT_THAT(
dynamic_joint_state_msg.interface_values[2].interface_names,
ElementsAreArray( interface_names_ ) );
}
TEST_F( JointStateBroadcasterTest, ActivateTestWithoutJointsParameter )
{
const std::vector<std::string> JOINT_NAMES = {};
const std::vector<std::string> IF_NAMES = {interface_names_[0]};
SetUpStateBroadcaster( JOINT_NAMES, IF_NAMES );
// configure ok
ASSERT_EQ( state_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
ASSERT_EQ( state_broadcaster_->on_activate( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
const size_t NUM_JOINTS = joint_names_.size( );
// publishers initialized
ASSERT_TRUE( state_broadcaster_->realtime_joint_state_publisher_ );
ASSERT_TRUE( state_broadcaster_->realtime_dynamic_joint_state_publisher_ );
// joint state initialized
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
ASSERT_THAT( joint_state_msg.name, ElementsAreArray( joint_names_ ) );
ASSERT_THAT( joint_state_msg.position, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( joint_state_msg.velocity, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( joint_state_msg.effort, SizeIs( NUM_JOINTS ) );
// dynamic joint state initialized
const auto & dynamic_joint_state_msg =
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
ASSERT_THAT( dynamic_joint_state_msg.joint_names, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( dynamic_joint_state_msg.interface_values, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( dynamic_joint_state_msg.joint_names, ElementsAreArray( joint_names_ ) );
ASSERT_THAT(
dynamic_joint_state_msg.interface_values[0].interface_names,
ElementsAreArray( interface_names_ ) );
ASSERT_THAT(
dynamic_joint_state_msg.interface_values[1].interface_names,
ElementsAreArray( interface_names_ ) );
ASSERT_THAT(
dynamic_joint_state_msg.interface_values[2].interface_names,
ElementsAreArray( interface_names_ ) );
}
TEST_F( JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter )
{
const std::vector<std::string> JOINT_NAMES = {"joint1"};
const std::vector<std::string> IF_NAMES = {};
SetUpStateBroadcaster( JOINT_NAMES, IF_NAMES );
// configure ok
ASSERT_EQ( state_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
ASSERT_EQ( state_broadcaster_->on_activate( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
const size_t NUM_JOINTS = joint_names_.size( );
// publishers initialized
ASSERT_TRUE( state_broadcaster_->realtime_joint_state_publisher_ );
ASSERT_TRUE( state_broadcaster_->realtime_dynamic_joint_state_publisher_ );
// joint state initialized
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
ASSERT_THAT( joint_state_msg.name, ElementsAreArray( joint_names_ ) );
ASSERT_THAT( joint_state_msg.position, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( joint_state_msg.velocity, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( joint_state_msg.effort, SizeIs( NUM_JOINTS ) );
// dynamic joint state initialized
const auto & dynamic_joint_state_msg =
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
ASSERT_THAT( dynamic_joint_state_msg.joint_names, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( dynamic_joint_state_msg.interface_values, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( dynamic_joint_state_msg.joint_names, ElementsAreArray( joint_names_ ) );
ASSERT_THAT(
dynamic_joint_state_msg.interface_values[0].interface_names,
ElementsAreArray( interface_names_ ) );
ASSERT_THAT(
dynamic_joint_state_msg.interface_values[1].interface_names,
ElementsAreArray( interface_names_ ) );
ASSERT_THAT(
dynamic_joint_state_msg.interface_values[2].interface_names,
ElementsAreArray( interface_names_ ) );
}
TEST_F( JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface )
{
const std::vector<std::string> JOINT_NAMES = {joint_names_[0], joint_names_[1]};
const std::vector<std::string> IF_NAMES = {interface_names_[0]};
SetUpStateBroadcaster( JOINT_NAMES, IF_NAMES );
// configure ok
ASSERT_EQ( state_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
ASSERT_EQ( state_broadcaster_->on_activate( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
const size_t NUM_JOINTS = JOINT_NAMES.size( );
// publishers initialized
ASSERT_TRUE( state_broadcaster_->realtime_joint_state_publisher_ );
ASSERT_TRUE( state_broadcaster_->realtime_dynamic_joint_state_publisher_ );
// joint state initialized
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
ASSERT_THAT( joint_state_msg.name, ElementsAreArray( JOINT_NAMES ) );
ASSERT_THAT( joint_state_msg.position, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( joint_state_msg.velocity, SizeIs( NUM_JOINTS ) );
for ( auto i = 0ul; i < NUM_JOINTS; ++i )
{
ASSERT_TRUE( std::isnan( joint_state_msg.velocity[i] ) );
}
ASSERT_THAT( joint_state_msg.effort, SizeIs( NUM_JOINTS ) );
for ( auto i = 0ul; i < NUM_JOINTS; ++i )
{
ASSERT_TRUE( std::isnan( joint_state_msg.effort[i] ) );
}
// dynamic joint state initialized
const auto & dynamic_joint_state_msg =
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
ASSERT_THAT( dynamic_joint_state_msg.joint_names, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( dynamic_joint_state_msg.interface_values, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( dynamic_joint_state_msg.joint_names, ElementsAreArray( JOINT_NAMES ) );
ASSERT_THAT(
dynamic_joint_state_msg.interface_values[0].interface_names, ElementsAreArray( IF_NAMES ) );
ASSERT_THAT(
dynamic_joint_state_msg.interface_values[1].interface_names, ElementsAreArray( IF_NAMES ) );
}
TEST_F( JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces )
{
const std::vector<std::string> JOINT_NAMES = {joint_names_[0]};
const std::vector<std::string> IF_NAMES = {interface_names_[0], interface_names_[1]};
SetUpStateBroadcaster( JOINT_NAMES, IF_NAMES );
// configure ok
ASSERT_EQ( state_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
ASSERT_EQ( state_broadcaster_->on_activate( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
const size_t NUM_JOINTS = JOINT_NAMES.size( );
// publishers initialized
ASSERT_TRUE( state_broadcaster_->realtime_joint_state_publisher_ );
ASSERT_TRUE( state_broadcaster_->realtime_dynamic_joint_state_publisher_ );
// joint state initialized
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
ASSERT_THAT( joint_state_msg.name, ElementsAreArray( JOINT_NAMES ) );
ASSERT_THAT( joint_state_msg.position, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( joint_state_msg.velocity, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( joint_state_msg.effort, SizeIs( NUM_JOINTS ) );
for ( auto i = 0ul; i < NUM_JOINTS; ++i )
{
ASSERT_TRUE( std::isnan( joint_state_msg.effort[i] ) );
}
// dynamic joint state initialized
const auto & dynamic_joint_state_msg =
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
ASSERT_THAT( dynamic_joint_state_msg.joint_names, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( dynamic_joint_state_msg.interface_values, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( dynamic_joint_state_msg.joint_names, ElementsAreArray( JOINT_NAMES ) );
ASSERT_THAT(
dynamic_joint_state_msg.interface_values[0].interface_names, ElementsAreArray( IF_NAMES ) );
}
TEST_F( JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesAllMissing )
{
const std::vector<std::string> JOINT_NAMES = {joint_names_[0], joint_names_[1]};
const std::vector<std::string> IF_NAMES = {interface_names_[0], interface_names_[1]};
init_broadcaster_and_set_parameters( JOINT_NAMES, {interface_names_[0], interface_names_[1]} );
// assign state with interfaces which are not set in parameters --> We should actually not assign
// anything because CM will also not do that
// assign_state_interfaces( JOINT_NAMES, {interface_names_[2]} );
// configure ok
ASSERT_EQ( state_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
// is none of requested interfaces do not exist, the controller will not be activated
ASSERT_EQ( state_broadcaster_->on_activate( rclcpp_lifecycle::State( ) ), NODE_ERROR );
}
TEST_F( JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing )
{
const std::vector<std::string> JOINT_NAMES = {joint_names_[0], joint_names_[1]};
const std::vector<std::string> IF_NAMES = {interface_names_[0], interface_names_[1]};
init_broadcaster_and_set_parameters( JOINT_NAMES, {interface_names_[0], interface_names_[1]} );
// Manually assign existing interfaces --> one we need is missing
std::vector<LoanedStateInterface> state_ifs;
state_ifs.emplace_back( joint_1_pos_state_ );
// Missing Joint 1 vel state interface
state_ifs.emplace_back( joint_2_pos_state_ );
state_ifs.emplace_back( joint_2_vel_state_ );
state_broadcaster_->assign_interfaces( {}, std::move( state_ifs ) );
// configure ok
ASSERT_EQ( state_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
// here a warning output is expected!
ASSERT_EQ( state_broadcaster_->on_activate( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
const size_t NUM_JOINTS = JOINT_NAMES.size( );
// publishers initialized
ASSERT_TRUE( state_broadcaster_->realtime_joint_state_publisher_ );
ASSERT_TRUE( state_broadcaster_->realtime_dynamic_joint_state_publisher_ );
// joint state initialized
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
ASSERT_THAT( joint_state_msg.name, ElementsAreArray( JOINT_NAMES ) );
ASSERT_THAT( joint_state_msg.position, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( joint_state_msg.velocity, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( joint_state_msg.effort, SizeIs( NUM_JOINTS ) );
// velocity for joint 1 should be nan because state interface does not exit
ASSERT_TRUE( std::isnan( joint_state_msg.velocity[0] ) );
for ( auto i = 0ul; i < NUM_JOINTS; ++i )
{
ASSERT_TRUE( std::isnan( joint_state_msg.effort[i] ) );
}
// dynamic joint state initialized
const auto & dynamic_joint_state_msg =
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
ASSERT_THAT( dynamic_joint_state_msg.joint_names, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( dynamic_joint_state_msg.interface_values, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( dynamic_joint_state_msg.joint_names, ElementsAreArray( JOINT_NAMES ) );
ASSERT_THAT(
dynamic_joint_state_msg.interface_values[0].interface_names,
ElementsAreArray( {IF_NAMES[0]} ) ); // joint 1 has only pos interface
ASSERT_THAT(
dynamic_joint_state_msg.interface_values[1].interface_names, ElementsAreArray( IF_NAMES ) );
}
TEST_F( JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping )
{
const std::vector<std::string> JOINT_NAMES = {joint_names_[0]};
const std::vector<std::string> IF_NAMES = {custom_interface_name_};
SetUpStateBroadcaster( JOINT_NAMES, IF_NAMES );
// configure ok
ASSERT_EQ( state_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
ASSERT_EQ( state_broadcaster_->on_activate( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
const size_t NUM_JOINTS = JOINT_NAMES.size( );
// joint state initialized
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
ASSERT_THAT( joint_state_msg.name, SizeIs( 0 ) );
ASSERT_THAT( joint_state_msg.position, SizeIs( 0 ) );
ASSERT_THAT( joint_state_msg.velocity, SizeIs( 0 ) );
ASSERT_THAT( joint_state_msg.effort, SizeIs( 0 ) );
// dynamic joint state initialized
const auto & dynamic_joint_state_msg =
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
ASSERT_THAT( dynamic_joint_state_msg.joint_names, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( dynamic_joint_state_msg.interface_values, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( dynamic_joint_state_msg.joint_names, ElementsAreArray( JOINT_NAMES ) );
ASSERT_THAT(
dynamic_joint_state_msg.interface_values[0].interface_names, ElementsAreArray( IF_NAMES ) );
// publishers initialized
ASSERT_TRUE( state_broadcaster_->joint_state_publisher_ );
ASSERT_TRUE( state_broadcaster_->dynamic_joint_state_publisher_ );
}
TEST_F( JointStateBroadcasterTest, TestCustomInterfaceMapping )
{
const std::vector<std::string> JOINT_NAMES = {joint_names_[0]};
const std::vector<std::string> IF_NAMES = {custom_interface_name_};
SetUpStateBroadcaster( JOINT_NAMES, IF_NAMES );
state_broadcaster_->get_node( )->set_parameter(
{std::string( "map_interface_to_joint_state." ) + HW_IF_POSITION, custom_interface_name_} );
// configure ok
ASSERT_EQ( state_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
ASSERT_EQ( state_broadcaster_->on_activate( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
const size_t NUM_JOINTS = JOINT_NAMES.size( );
// joint state initialized
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
ASSERT_THAT( joint_state_msg.name, ElementsAreArray( JOINT_NAMES ) );
ASSERT_THAT( joint_state_msg.position, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( joint_state_msg.velocity, SizeIs( NUM_JOINTS ) );
for ( auto i = 0ul; i < NUM_JOINTS; ++i )
{
ASSERT_TRUE( std::isnan( joint_state_msg.velocity[i] ) );
}
ASSERT_THAT( joint_state_msg.effort, SizeIs( NUM_JOINTS ) );
for ( auto i = 0ul; i < NUM_JOINTS; ++i )
{
ASSERT_TRUE( std::isnan( joint_state_msg.effort[i] ) );
}
// dynamic joint state initialized
const auto & dynamic_joint_state_msg =
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
ASSERT_THAT( dynamic_joint_state_msg.joint_names, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( dynamic_joint_state_msg.interface_values, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( dynamic_joint_state_msg.joint_names, ElementsAreArray( JOINT_NAMES ) );
ASSERT_THAT(
dynamic_joint_state_msg.interface_values[0].interface_names,
ElementsAreArray( {HW_IF_POSITION} ) ); // mapping to this value
// publishers initialized
ASSERT_TRUE( state_broadcaster_->joint_state_publisher_ );
ASSERT_TRUE( state_broadcaster_->dynamic_joint_state_publisher_ );
}
TEST_F( JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate )
{
const std::vector<std::string> JOINT_NAMES = {joint_names_[0]};
const std::vector<std::string> IF_NAMES = {custom_interface_name_};
SetUpStateBroadcaster( JOINT_NAMES, IF_NAMES );
state_broadcaster_->get_node( )->set_parameter(
{std::string( "map_interface_to_joint_state." ) + HW_IF_POSITION, custom_interface_name_} );
// configure ok
ASSERT_EQ( state_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
ASSERT_EQ( state_broadcaster_->on_activate( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
ASSERT_EQ(
state_broadcaster_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
const size_t NUM_JOINTS = JOINT_NAMES.size( );
// joint state initialized
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
ASSERT_THAT( joint_state_msg.name, ElementsAreArray( JOINT_NAMES ) );
ASSERT_THAT( joint_state_msg.position, SizeIs( NUM_JOINTS ) );
ASSERT_EQ( joint_state_msg.position[0], custom_joint_value_ );
ASSERT_THAT( joint_state_msg.velocity, SizeIs( NUM_JOINTS ) );
for ( auto i = 0ul; i < NUM_JOINTS; ++i )
{
ASSERT_TRUE( std::isnan( joint_state_msg.velocity[i] ) );
}
ASSERT_THAT( joint_state_msg.effort, SizeIs( NUM_JOINTS ) );
for ( auto i = 0ul; i < NUM_JOINTS; ++i )
{
ASSERT_TRUE( std::isnan( joint_state_msg.effort[i] ) );
}
// dynamic joint state initialized
const auto & dynamic_joint_state_msg =
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
ASSERT_THAT( dynamic_joint_state_msg.joint_names, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( dynamic_joint_state_msg.interface_values, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( dynamic_joint_state_msg.joint_names, ElementsAreArray( JOINT_NAMES ) );
ASSERT_THAT(
dynamic_joint_state_msg.interface_values[0].interface_names,
ElementsAreArray( {HW_IF_POSITION} ) );
// publishers initialized
ASSERT_TRUE( state_broadcaster_->joint_state_publisher_ );
ASSERT_TRUE( state_broadcaster_->dynamic_joint_state_publisher_ );
}
TEST_F( JointStateBroadcasterTest, UpdateTest )
{
SetUpStateBroadcaster( );
auto node_state = state_broadcaster_->get_node( )->configure( );
node_state = state_broadcaster_->get_node( )->activate( );
ASSERT_EQ( node_state.id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
ASSERT_EQ(
state_broadcaster_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
}
void JointStateBroadcasterTest::test_published_joint_state_message( const std::string & topic )
{
auto node_state = state_broadcaster_->get_node( )->configure( );
ASSERT_EQ( node_state.id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
node_state = state_broadcaster_->get_node( )->activate( );
ASSERT_EQ( node_state.id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
rclcpp::Node test_node( "test_node" );
auto subs_callback = [&]( const sensor_msgs::msg::JointState::SharedPtr ) {};
auto subscription =
test_node.create_subscription<sensor_msgs::msg::JointState>( topic, 10, subs_callback );
// call update to publish the test value
// since update doesn't guarantee a published message, republish until received
int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop
rclcpp::WaitSet wait_set; // block used to wait on message
wait_set.add_subscription( subscription );
while ( max_sub_check_loop_count-- )
{
state_broadcaster_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
// check if message has been received
if ( wait_set.wait( std::chrono::milliseconds( 2 ) ).kind( ) == rclcpp::WaitResultKind::Ready )
{
break;
}
}
ASSERT_GE( max_sub_check_loop_count, 0 ) << "Test was unable to publish a message through "
"controller/broadcaster update loop";
// take message from subscription
sensor_msgs::msg::JointState joint_state_msg;
rclcpp::MessageInfo msg_info;
ASSERT_TRUE( subscription->take( joint_state_msg, msg_info ) );
const size_t NUM_JOINTS = joint_names_.size( );
ASSERT_THAT( joint_state_msg.name, SizeIs( NUM_JOINTS ) );
// the order in the message may be different
// we only check that all values in this test are present in the message
// and that it is the same across the interfaces
// for test purposes they are mapped to the same doubles
ASSERT_THAT( joint_state_msg.position, ElementsAreArray( joint_values_ ) );
ASSERT_THAT( joint_state_msg.velocity, ElementsAreArray( joint_state_msg.position ) );
ASSERT_THAT( joint_state_msg.effort, ElementsAreArray( joint_state_msg.position ) );
}
TEST_F( JointStateBroadcasterTest, JointStatePublishTest )
{
SetUpStateBroadcaster( );
test_published_joint_state_message( "joint_states" );
}
TEST_F( JointStateBroadcasterTest, JointStatePublishTestLocalTopic )
{
SetUpStateBroadcaster( );
state_broadcaster_->get_node( )->set_parameter( {"use_local_topics", true} );
test_published_joint_state_message( "joint_state_broadcaster/joint_states" );
}
void JointStateBroadcasterTest::test_published_dynamic_joint_state_message(
const std::string & topic )
{
auto node_state = state_broadcaster_->get_node( )->configure( );
ASSERT_EQ( node_state.id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
node_state = state_broadcaster_->get_node( )->activate( );
ASSERT_EQ( node_state.id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
rclcpp::Node test_node( "test_node" );
auto subs_callback = [&]( const control_msgs::msg::DynamicJointState::SharedPtr ) {};
auto subscription =
test_node.create_subscription<control_msgs::msg::DynamicJointState>( topic, 10, subs_callback );
// call update to publish the test value
// since update doesn't guarantee a published message, republish until received
int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop
rclcpp::WaitSet wait_set; // block used to wait on message
wait_set.add_subscription( subscription );
while ( max_sub_check_loop_count-- )
{
state_broadcaster_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
// check if message has been received
if ( wait_set.wait( std::chrono::milliseconds( 2 ) ).kind( ) == rclcpp::WaitResultKind::Ready )
{
break;
}
}
ASSERT_GE( max_sub_check_loop_count, 0 ) << "Test was unable to publish a message through "
"controller/broadcaster update loop";
// take message from subscription
control_msgs::msg::DynamicJointState dynamic_joint_state_msg;
rclcpp::MessageInfo msg_info;
ASSERT_TRUE( subscription->take( dynamic_joint_state_msg, msg_info ) );
const size_t NUM_JOINTS = 3;
const auto INTERFACE_NAMES = {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT};
ASSERT_THAT( dynamic_joint_state_msg.joint_names, SizeIs( NUM_JOINTS ) );
// the order in the message may be different
// we only check that all values in this test are present in the message
// and that it is the same across the interfaces
// for test purposes they are mapped to the same doubles
for ( size_t i = 0; i < dynamic_joint_state_msg.joint_names.size( ); ++i )
{
ASSERT_THAT(
dynamic_joint_state_msg.interface_values[i].interface_names,
ElementsAreArray( INTERFACE_NAMES ) );
const auto index_in_original_order = std::distance(
joint_names_.cbegin( ),
std::find(
joint_names_.cbegin( ), joint_names_.cend( ), dynamic_joint_state_msg.joint_names[i] ) );
ASSERT_THAT(
dynamic_joint_state_msg.interface_values[i].values,
Each( joint_values_[index_in_original_order] ) );
}
}
TEST_F( JointStateBroadcasterTest, DynamicJointStatePublishTest )
{
SetUpStateBroadcaster( );
test_published_dynamic_joint_state_message( "dynamic_joint_states" );
}
TEST_F( JointStateBroadcasterTest, DynamicJointStatePublishTestLocalTopic )
{
SetUpStateBroadcaster( );
state_broadcaster_->get_node( )->set_parameter( {"use_local_topics", true} );
test_published_dynamic_joint_state_message( "joint_state_broadcaster/dynamic_joint_states" );
}
TEST_F( JointStateBroadcasterTest, ExtraJointStatePublishTest )
{
// publishers not initialized yet
ASSERT_FALSE( state_broadcaster_->realtime_joint_state_publisher_ );
ASSERT_FALSE( state_broadcaster_->realtime_dynamic_joint_state_publisher_ );
SetUpStateBroadcaster( );
// Add extra joints as parameters
const std::vector<std::string> extra_joint_names = {"extra1", "extra2", "extra3"};
state_broadcaster_->get_node( )->set_parameter( {"extra_joints", extra_joint_names} );
// configure ok
ASSERT_EQ( state_broadcaster_->on_configure( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
ASSERT_EQ( state_broadcaster_->on_activate( rclcpp_lifecycle::State( ) ), NODE_SUCCESS );
std::vector<std::string> all_joint_names = joint_names_;
all_joint_names.insert( all_joint_names.end( ), extra_joint_names.begin( ), extra_joint_names.end( ) );
const size_t NUM_JOINTS = all_joint_names.size( );
// joint state initialized
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
ASSERT_THAT( joint_state_msg.name, ElementsAreArray( all_joint_names ) );
ASSERT_THAT( joint_state_msg.position, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( joint_state_msg.velocity, SizeIs( NUM_JOINTS ) );
ASSERT_THAT( joint_state_msg.effort, SizeIs( NUM_JOINTS ) );
// dynamic joint state initialized
const auto & dynamic_joint_state_msg =
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
ASSERT_THAT( dynamic_joint_state_msg.joint_names, SizeIs( NUM_JOINTS ) );
}
// Copyright 2020 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TEST_JOINT_STATE_BROADCASTER_HPP_
#define TEST_JOINT_STATE_BROADCASTER_HPP_
#include <memory>
#include <string>
#include <vector>
#include "gmock/gmock.h"
#include "joint_state_broadcaster/joint_state_broadcaster.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
using hardware_interface::HW_IF_EFFORT;
using hardware_interface::HW_IF_POSITION;
using hardware_interface::HW_IF_VELOCITY;
// subclassing and friending so we can access member variables
33 class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBroadcaster
{
35 FRIEND_TEST( JointStateBroadcasterTest, ConfigureErrorTest );
36 FRIEND_TEST( JointStateBroadcasterTest, ActivateTest );
37 FRIEND_TEST( JointStateBroadcasterTest, ActivateTestWithoutJointsParameter );
38 FRIEND_TEST( JointStateBroadcasterTest, ActivateTestWithoutInterfacesParameter );
39 FRIEND_TEST( JointStateBroadcasterTest, ActivateTestTwoJointsOneInterface );
40 FRIEND_TEST( JointStateBroadcasterTest, ActivateTestOneJointTwoInterfaces );
41 FRIEND_TEST( JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesAllMissing );
42 FRIEND_TEST( JointStateBroadcasterTest, ActivateTestTwoJointTwoInterfacesOneMissing );
43 FRIEND_TEST( JointStateBroadcasterTest, TestCustomInterfaceWithoutMapping );
44 FRIEND_TEST( JointStateBroadcasterTest, TestCustomInterfaceMapping );
45 FRIEND_TEST( JointStateBroadcasterTest, TestCustomInterfaceMappingUpdate );
46 FRIEND_TEST( JointStateBroadcasterTest, ExtraJointStatePublishTest );
};
49 class JointStateBroadcasterTest : public ::testing::Test
{
public:
52 static void SetUpTestCase( );
53 static void TearDownTestCase( );
55 void SetUp( );
56 void TearDown( );
58 void SetUpStateBroadcaster(
const std::vector<std::string> & joint_names = {},
const std::vector<std::string> & interfaces = {} );
62 void init_broadcaster_and_set_parameters(
const std::vector<std::string> & joint_names = {},
const std::vector<std::string> & interfaces = {} );
66 void assign_state_interfaces(
const std::vector<std::string> & joint_names = {},
const std::vector<std::string> & interfaces = {} );
70 void test_published_joint_state_message( const std::string & topic );
72 void test_published_dynamic_joint_state_message( const std::string & topic );
protected:
// dummy joint state values used for tests
76 const std::vector<std::string> joint_names_ = {"joint1", "joint2", "joint3"};
77 const std::vector<std::string> interface_names_ = {HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_EFFORT};
78 std::string custom_interface_name_ = "measured_position";
79 std::vector<double> joint_values_ = {1.1, 2.1, 3.1};
double custom_joint_value_ = 3.5;
hardware_interface::StateInterface joint_1_pos_state_{
joint_names_[0], interface_names_[0], &joint_values_[0]};
hardware_interface::StateInterface joint_2_pos_state_{
85 joint_names_[1], interface_names_[0], &joint_values_[1]};
hardware_interface::StateInterface joint_3_pos_state_{
joint_names_[2], interface_names_[0], &joint_values_[2]};
hardware_interface::StateInterface joint_1_vel_state_{
joint_names_[0], interface_names_[1], &joint_values_[0]};
hardware_interface::StateInterface joint_2_vel_state_{
joint_names_[1], interface_names_[1], &joint_values_[1]};
hardware_interface::StateInterface joint_3_vel_state_{
joint_names_[2], interface_names_[1], &joint_values_[2]};
hardware_interface::StateInterface joint_1_eff_state_{
joint_names_[0], interface_names_[2], &joint_values_[0]};
hardware_interface::StateInterface joint_2_eff_state_{
joint_names_[1], interface_names_[2], &joint_values_[1]};
hardware_interface::StateInterface joint_3_eff_state_{
joint_names_[2], interface_names_[2], &joint_values_[2]};
hardware_interface::StateInterface joint_X_custom_state{
joint_names_[0], custom_interface_name_, &custom_joint_value_};
std::unique_ptr<FriendJointStateBroadcaster> state_broadcaster_;
};
#endif // TEST_JOINT_STATE_BROADCASTER_HPP_
1 // Copyright 2020 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gmock/gmock.h>
#include <memory>
#include "controller_manager/controller_manager.hpp"
#include "hardware_interface/resource_manager.hpp"
#include "rclcpp/executor.hpp"
#include "rclcpp/executors/single_threaded_executor.hpp"
#include "rclcpp/utilities.hpp"
#include "ros2_control_test_assets/descriptions.hpp"
25 TEST( TestLoadJointStateBroadcaster, load_controller )
{
rclcpp::init( 0, nullptr );
std::shared_ptr<rclcpp::Executor> executor =
std::make_shared<rclcpp::executors::SingleThreadedExecutor>( );
controller_manager::ControllerManager cm(
std::make_unique<hardware_interface::ResourceManager>(
ros2_control_test_assets::minimal_robot_urdf ),
executor, "test_controller_manager" );
ASSERT_NO_THROW( cm.load_controller(
"test_joint_state_broadcaster", "joint_state_broadcaster/JointStateBroadcaster" ) );
rclcpp::shutdown( );
}
// Copyright ( c ) 2022 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef JOINT_TRAJECTORY_CONTROLLER__INTERPOLATION_METHODS_HPP_
#define JOINT_TRAJECTORY_CONTROLLER__INTERPOLATION_METHODS_HPP_
#include <rclcpp/rclcpp.hpp>
#include <string>
#include <unordered_map>
namespace joint_trajectory_controller
{
static const rclcpp::Logger LOGGER =
rclcpp::get_logger( "joint_trajectory_controller.interpolation_methods" );
namespace interpolation_methods
{
29 enum class InterpolationMethod
{
NONE,
VARIABLE_DEGREE_SPLINE
};
const InterpolationMethod DEFAULT_INTERPOLATION = InterpolationMethod::VARIABLE_DEGREE_SPLINE;
37 const std::unordered_map<InterpolationMethod, std::string> InterpolationMethodMap(
{{InterpolationMethod::NONE, "none"}, {InterpolationMethod::VARIABLE_DEGREE_SPLINE, "splines"}} );
40 [[nodiscard]] inline InterpolationMethod from_string( const std::string & interpolation_method )
{
if ( interpolation_method.compare( InterpolationMethodMap.at( InterpolationMethod::NONE ) ) == 0 )
{
return InterpolationMethod::NONE;
}
else if (
!interpolation_method.compare(
InterpolationMethodMap.at( InterpolationMethod::VARIABLE_DEGREE_SPLINE ) ) == 0 )
{
return InterpolationMethod::VARIABLE_DEGREE_SPLINE;
}
// Default
else
{
RCLCPP_INFO_STREAM(
LOGGER,
"No interpolation method parameter was given. Using the default, VARIABLE_DEGREE_SPLINE." );
return InterpolationMethod::VARIABLE_DEGREE_SPLINE;
}
}
} // namespace interpolation_methods
} // namespace joint_trajectory_controller
#endif // JOINT_TRAJECTORY_CONTROLLER__INTERPOLATION_METHODS_HPP_
1 // Copyright ( c ) 2021 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_
#define JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_
#include <chrono>
#include <memory>
#include <mutex>
#include <string>
#include <utility>
#include <vector>
#include "control_msgs/action/follow_joint_trajectory.hpp"
#include "control_msgs/msg/joint_trajectory_controller_state.hpp"
#include "control_toolbox/pid.hpp"
#include "controller_interface/controller_interface.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "joint_trajectory_controller/interpolation_methods.hpp"
#include "joint_trajectory_controller/tolerances.hpp"
#include "joint_trajectory_controller/visibility_control.h"
#include "rclcpp/duration.hpp"
#include "rclcpp/subscription.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp/timer.hpp"
#include "rclcpp_action/server.hpp"
#include "rclcpp_action/types.hpp"
#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "realtime_tools/realtime_buffer.h"
#include "realtime_tools/realtime_publisher.h"
#include "realtime_tools/realtime_server_goal_handle.h"
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
using namespace std::chrono_literals; // NOLINT
namespace rclcpp_action
{
template <typename ActionT>
52 class ServerGoalHandle;
} // namespace rclcpp_action
namespace rclcpp_lifecycle
{
56 class State;
} // namespace rclcpp_lifecycle
59 namespace joint_trajectory_controller
{
class Trajectory;
class JointTrajectoryController : public controller_interface::ControllerInterface
{
public:
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
JointTrajectoryController( );
/**
* @brief command_interface_configuration This controller requires the position command
* interfaces for the controlled joints
*/
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
controller_interface::InterfaceConfiguration command_interface_configuration( ) const override;
/**
* @brief command_interface_configuration This controller requires the position and velocity
* state interfaces for the controlled joints
*/
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
controller_interface::InterfaceConfiguration state_interface_configuration( ) const override;
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
controller_interface::return_type update(
const rclcpp::Time & time, const rclcpp::Duration & period ) override;
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_init( ) override;
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_configure(
const rclcpp_lifecycle::State & previous_state ) override;
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_activate(
const rclcpp_lifecycle::State & previous_state ) override;
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_deactivate(
const rclcpp_lifecycle::State & previous_state ) override;
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_cleanup(
const rclcpp_lifecycle::State & previous_state ) override;
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_error(
const rclcpp_lifecycle::State & previous_state ) override;
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
controller_interface::CallbackReturn on_shutdown(
const rclcpp_lifecycle::State & previous_state ) override;
protected:
std::vector<std::string> joint_names_;
std::vector<std::string> command_interface_types_;
std::vector<std::string> state_interface_types_;
// To reduce number of variables and to make the code shorter the interfaces are ordered in types
// as the following constants
const std::vector<std::string> allowed_interface_types_ = {
hardware_interface::HW_IF_POSITION,
hardware_interface::HW_IF_VELOCITY,
hardware_interface::HW_IF_ACCELERATION,
hardware_interface::HW_IF_EFFORT,
};
// Preallocate variables used in the realtime update( ) function
trajectory_msgs::msg::JointTrajectoryPoint state_current_;
trajectory_msgs::msg::JointTrajectoryPoint state_desired_;
trajectory_msgs::msg::JointTrajectoryPoint state_error_;
// Degrees of freedom
size_t dof_;
// Parameters for some special cases, e.g. hydraulics powered robots
// Run the controller in open-loop, i.e., read hardware states only when starting controller.
// This is useful when robot is not exactly following the commanded trajectory.
bool open_loop_control_ = false;
trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_;
/// Allow integration in goal trajectories to accept goals without position or velocity specified
bool allow_integration_in_goal_trajectories_ = false;
/// Specify interpolation method. Default to splines.
interpolation_methods::InterpolationMethod interpolation_method_{
interpolation_methods::DEFAULT_INTERPOLATION};
double state_publish_rate_;
double action_monitor_rate_;
// The interfaces are defined as the types in 'allowed_interface_types_' member.
// For convenience, for each type the interfaces are ordered so that i-th position
// matches i-th index in joint_names_
template <typename T>
using InterfaceReferences = std::vector<std::vector<std::reference_wrapper<T>>>;
InterfaceReferences<hardware_interface::LoanedCommandInterface> joint_command_interface_;
InterfaceReferences<hardware_interface::LoanedStateInterface> joint_state_interface_;
bool has_position_state_interface_ = false;
bool has_velocity_state_interface_ = false;
bool has_acceleration_state_interface_ = false;
bool has_position_command_interface_ = false;
bool has_velocity_command_interface_ = false;
bool has_acceleration_command_interface_ = false;
bool has_effort_command_interface_ = false;
/// If true, a velocity feedforward term plus corrective PID term is used
bool use_closed_loop_pid_adapter_ = false;
using PidPtr = std::shared_ptr<control_toolbox::Pid>;
std::vector<PidPtr> pids_;
// Feed-forward velocity weight factor when calculating closed loop pid adapter's command
std::vector<double> ff_velocity_scale_;
// reserved storage for result of the command when closed loop pid adapter is used
std::vector<double> tmp_command_;
// TODO( karsten1987 ): eventually activate and deactivate subscriber directly when its supported
bool subscriber_is_active_ = false;
rclcpp::Subscription<trajectory_msgs::msg::JointTrajectory>::SharedPtr joint_command_subscriber_ =
nullptr;
std::shared_ptr<Trajectory> * traj_point_active_ptr_ = nullptr;
std::shared_ptr<Trajectory> traj_external_point_ptr_ = nullptr;
std::shared_ptr<Trajectory> traj_home_point_ptr_ = nullptr;
std::shared_ptr<trajectory_msgs::msg::JointTrajectory> traj_msg_home_ptr_ = nullptr;
realtime_tools::RealtimeBuffer<std::shared_ptr<trajectory_msgs::msg::JointTrajectory>>
traj_msg_external_point_ptr_;
using ControllerStateMsg = control_msgs::msg::JointTrajectoryControllerState;
using StatePublisher = realtime_tools::RealtimePublisher<ControllerStateMsg>;
using StatePublisherPtr = std::unique_ptr<StatePublisher>;
rclcpp::Publisher<ControllerStateMsg>::SharedPtr publisher_;
StatePublisherPtr state_publisher_;
rclcpp::Duration state_publisher_period_ = rclcpp::Duration( 20ms );
rclcpp::Time last_state_publish_time_;
using FollowJTrajAction = control_msgs::action::FollowJointTrajectory;
using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle<FollowJTrajAction>;
using RealtimeGoalHandlePtr = std::shared_ptr<RealtimeGoalHandle>;
using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer<RealtimeGoalHandlePtr>;
rclcpp_action::Server<FollowJTrajAction>::SharedPtr action_server_;
bool allow_partial_joints_goal_ = false;
RealtimeGoalHandleBuffer rt_active_goal_; ///< Currently active action goal, if any.
rclcpp::TimerBase::SharedPtr goal_handle_timer_;
rclcpp::Duration action_monitor_period_ = rclcpp::Duration( 50ms );
// callbacks for action_server_
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
rclcpp_action::GoalResponse goal_callback(
const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const FollowJTrajAction::Goal> goal );
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
rclcpp_action::CancelResponse cancel_callback(
const std::shared_ptr<rclcpp_action::ServerGoalHandle<FollowJTrajAction>> goal_handle );
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
void feedback_setup_callback(
std::shared_ptr<rclcpp_action::ServerGoalHandle<FollowJTrajAction>> goal_handle );
// fill trajectory_msg so it matches joints controlled by this controller
// positions set to current position, velocities, accelerations and efforts to 0.0
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
void fill_partial_goal(
std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg ) const;
// sorts the joints of the incoming message to our local order
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
void sort_to_local_joint_order(
std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg );
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
bool validate_trajectory_msg( const trajectory_msgs::msg::JointTrajectory & trajectory ) const;
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
void add_new_trajectory_msg(
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg );
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
bool validate_trajectory_point_field(
size_t joint_names_size, const std::vector<double> & vector_field,
const std::string & string_for_vector_field, size_t i, bool allow_empty ) const;
SegmentTolerances default_tolerances_;
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
void preempt_active_goal( );
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
void set_hold_position( );
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
bool reset( );
using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint;
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
void publish_state(
const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state,
const JointTrajectoryPoint & state_error );
void read_state_from_hardware( JointTrajectoryPoint & state );
bool read_state_from_command_interfaces( JointTrajectoryPoint & state );
private:
bool contains_interface_type(
const std::vector<std::string> & interface_type_list, const std::string & interface_type );
void resize_joint_trajectory_point(
trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size );
};
} // namespace joint_trajectory_controller
#endif // JOINT_TRAJECTORY_CONTROLLER__JOINT_TRAJECTORY_CONTROLLER_HPP_
1 // Copyright 2013 PAL Robotics S.L.
// All rights reserved.
//
// Software License Agreement ( BSD License 2.0 )
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of PAL Robotics S.L. nor the names of its
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES ( INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION ) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT ( INCLUDING NEGLIGENCE OR OTHERWISE )
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
/// \author Adolfo Rodriguez Tsouroukdissian
#ifndef JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_
#define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_
#include <cassert>
#include <cmath>
#include <string>
#include <vector>
#include "control_msgs/action/follow_joint_trajectory.hpp"
#include "rclcpp/node.hpp"
namespace joint_trajectory_controller
{
/**
* \brief Trajectory state tolerances for position, velocity and acceleration variables.
*
* A tolerance value of zero means that no tolerance will be applied for that variable.
*/
struct StateTolerances
{
double position = 0.0;
double velocity = 0.0;
double acceleration = 0.0;
};
/**
* \brief Trajectory segment tolerances.
*/
struct SegmentTolerances
{
explicit SegmentTolerances( size_t size = 0 ) : state_tolerance( size ), goal_state_tolerance( size ) {}
/** State tolerances that apply during segment execution. */
std::vector<StateTolerances> state_tolerance;
/** State tolerances that apply for the goal state only.*/
std::vector<StateTolerances> goal_state_tolerance;
/** Extra time after the segment end time allowed to reach the goal state tolerances. */
double goal_time_tolerance = 0.0;
};
/**
* \brief Populate trajectory segment tolerances using data from the ROS node.
*
* It is assumed that the following parameter structure is followed on the provided LifecycleNode.
* Unspecified parameters will take the defaults shown in the comments:
*
* \code
* constraints:
* goal_time: 1.0 # Defaults to zero
* stopped_velocity_tolerance: 0.02 # Defaults to 0.01
* foo_joint:
* trajectory: 0.05 # Defaults to zero ( ie. the tolerance is not enforced )
* goal: 0.03 # Defaults to zero ( ie. the tolerance is not enforced )
* bar_joint:
* goal: 0.01
* \endcode
*
* \param node LifecycleNode where the tolerances are specified.
* \param joint_names Names of joints to look for in the parameter server for a tolerance specification.
* \return Trajectory segment tolerances.
*/
94 SegmentTolerances get_segment_tolerances(
const rclcpp_lifecycle::LifecycleNode & node, const std::vector<std::string> & joint_names )
{
const auto n_joints = joint_names.size( );
SegmentTolerances tolerances;
// State and goal state tolerances
double stopped_velocity_tolerance = 0.01;
node.get_parameter_or<double>(
"constraints.stopped_velocity_tolerance", stopped_velocity_tolerance,
stopped_velocity_tolerance );
tolerances.state_tolerance.resize( n_joints );
tolerances.goal_state_tolerance.resize( n_joints );
for ( size_t i = 0; i < n_joints; ++i )
{
const std::string prefix = "constraints." + joint_names[i];
node.get_parameter_or<double>(
prefix + ".trajectory", tolerances.state_tolerance[i].position, 0.0 );
node.get_parameter_or<double>(
prefix + ".goal", tolerances.goal_state_tolerance[i].position, 0.0 );
auto logger = rclcpp::get_logger( "tolerance" );
RCLCPP_DEBUG(
logger, "%s %f", ( prefix + ".trajectory" ).c_str( ), tolerances.state_tolerance[i].position );
RCLCPP_DEBUG(
logger, "%s %f", ( prefix + ".goal" ).c_str( ), tolerances.goal_state_tolerance[i].position );
tolerances.goal_state_tolerance[i].velocity = stopped_velocity_tolerance;
}
// Goal time tolerance
node.get_parameter_or<double>( "constraints.goal_time", tolerances.goal_time_tolerance, 0.0 );
return tolerances;
}
/**
* \param state_error State error to check.
* \param joint_idx Joint index for the state error
* \param state_tolerance State tolerance of joint to check \p state_error against.
* \param show_errors If the joint that violate its tolerance should be output to console. NOT REALTIME if true
* \return True if \p state_error fulfills \p state_tolerance.
*/
139 inline bool check_state_tolerance_per_joint(
const trajectory_msgs::msg::JointTrajectoryPoint & state_error, int joint_idx,
const StateTolerances & state_tolerance, bool show_errors = false )
{
using std::abs;
const double error_position = state_error.positions[joint_idx];
const double error_velocity =
state_error.velocities.empty( ) ? 0.0 : state_error.velocities[joint_idx];
const double error_acceleration =
state_error.accelerations.empty( ) ? 0.0 : state_error.accelerations[joint_idx];
const bool is_valid =
!( state_tolerance.position > 0.0 && abs( error_position ) > state_tolerance.position ) &&
!( state_tolerance.velocity > 0.0 && abs( error_velocity ) > state_tolerance.velocity ) &&
!( state_tolerance.acceleration > 0.0 && abs( error_acceleration ) > state_tolerance.acceleration );
if ( is_valid )
{
return true;
}
if ( show_errors )
{
const auto logger = rclcpp::get_logger( "tolerances" );
RCLCPP_ERROR( logger, "Path state tolerances failed:" );
if ( state_tolerance.position > 0.0 && abs( error_position ) > state_tolerance.position )
{
RCLCPP_ERROR(
logger, "Position Error: %f, Position Tolerance: %f", error_position,
state_tolerance.position );
}
if ( state_tolerance.velocity > 0.0 && abs( error_velocity ) > state_tolerance.velocity )
{
RCLCPP_ERROR(
logger, "Velocity Error: %f, Velocity Tolerance: %f", error_velocity,
state_tolerance.velocity );
}
if (
state_tolerance.acceleration > 0.0 && abs( error_acceleration ) > state_tolerance.acceleration )
{
RCLCPP_ERROR(
logger, "Acceleration Error: %f, Acceleration Tolerance: %f", error_acceleration,
state_tolerance.acceleration );
}
}
return false;
}
} // namespace joint_trajectory_controller
#endif // JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_
// Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_
#define JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_
#include <memory>
#include <vector>
#include "joint_trajectory_controller/interpolation_methods.hpp"
#include "joint_trajectory_controller/visibility_control.h"
#include "rclcpp/time.hpp"
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
namespace joint_trajectory_controller
{
using TrajectoryPointIter = std::vector<trajectory_msgs::msg::JointTrajectoryPoint>::iterator;
using TrajectoryPointConstIter =
std::vector<trajectory_msgs::msg::JointTrajectoryPoint>::const_iterator;
32 class Trajectory
{
public:
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
36 Trajectory( );
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
explicit Trajectory( std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory );
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
explicit Trajectory(
const rclcpp::Time & current_time,
const trajectory_msgs::msg::JointTrajectoryPoint & current_point,
std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory );
/// Set the point before the trajectory message is replaced/appended
/// Example: if we receive a new trajectory message and it's first point is 0.5 seconds
/// from the current one, we call this function to log the current state, then
/// append/replace the current trajectory
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
void set_point_before_trajectory_msg(
const rclcpp::Time & current_time,
const trajectory_msgs::msg::JointTrajectoryPoint & current_point );
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
void update( std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory );
/// Find the segment ( made up of 2 points ) and its expected state from the
/// containing trajectory.
/**
* Sampling trajectory at given \p sample_time.
* If position in the \p end_segment_itr is missing it will be deduced from provided velocity, or acceleration respectively.
* Deduction assumes that the provided velocity or acceleration have to be reached at the time defined in the segment.
*
* Specific case returns for start_segment_itr and end_segment_itr:
* - Sampling before the trajectory start:
* start_segment_itr = begin( ), end_segment_itr = begin( )
* - Sampling exactly on a point of the trajectory:
* start_segment_itr = iterator where point is, end_segment_itr = iterator after start_segment_itr
* - Sampling between points:
* start_segment_itr = iterator before the sampled point, end_segment_itr = iterator after start_segment_itr
* - Sampling after entire trajectory:
* start_segment_itr = --end( ), end_segment_itr = end( )
* - Sampling empty msg or before the time given in set_point_before_trajectory_msg( )
* return false
*
* \param[in] sample_time Time at which trajectory will be sampled.
* \param[in] interpolation_method Specify whether splines, another method, or no interpolation at all.
* \param[out] expected_state Calculated new at \p sample_time.
* \param[out] start_segment_itr Iterator to the start segment for given \p sample_time. See description above.
* \param[out] end_segment_itr Iterator to the end segment for given \p sample_time. See description above.
*/
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
bool sample(
const rclcpp::Time & sample_time,
const interpolation_methods::InterpolationMethod interpolation_method,
trajectory_msgs::msg::JointTrajectoryPoint & output_state,
TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr );
/**
* Do interpolation between 2 states given a time in between their respective timestamps
*
* The start and end states need not necessarily be specified all the way to the acceleration level:
* - If only \b positions are specified, linear interpolation will be used.
* - If \b positions and \b velocities are specified, a cubic spline will be used.
* - If \b positions, \b velocities and \b accelerations are specified, a quintic spline will be used.
*
* If start and end states have different specifications
* ( eg. start is position-only, end is position-velocity ), the lowest common specification will be used
* ( position-only in the example ).
*
* \param[in] time_a Time at which the segment state equals \p state_a.
* \param[in] state_a State at \p time_a.
* \param[in] time_b Time at which the segment state equals \p state_b.
* \param[in] state_b State at time \p time_b.
* \param[in] sample_time The time to sample, between time_a and time_b.
* \param[out] output The state at \p sample_time.
*/
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
void interpolate_between_points(
const rclcpp::Time & time_a, const trajectory_msgs::msg::JointTrajectoryPoint & state_a,
const rclcpp::Time & time_b, const trajectory_msgs::msg::JointTrajectoryPoint & state_b,
const rclcpp::Time & sample_time, trajectory_msgs::msg::JointTrajectoryPoint & output );
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
TrajectoryPointConstIter begin( ) const;
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
TrajectoryPointConstIter end( ) const;
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
rclcpp::Time time_from_start( ) const;
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
bool has_trajectory_msg( ) const;
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
std::shared_ptr<trajectory_msgs::msg::JointTrajectory> get_trajectory_msg( ) const
{
return trajectory_msg_;
}
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
135 rclcpp::Time get_trajectory_start_time( ) const { return trajectory_start_time_; }
137 JOINT_TRAJECTORY_CONTROLLER_PUBLIC
bool is_sampled_already( ) const { return sampled_already_; }
private:
141 void deduce_from_derivatives(
trajectory_msgs::msg::JointTrajectoryPoint & first_state,
trajectory_msgs::msg::JointTrajectoryPoint & second_state, const size_t dim,
const double delta_t );
std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg_;
rclcpp::Time trajectory_start_time_;
rclcpp::Time time_before_traj_msg_;
trajectory_msgs::msg::JointTrajectoryPoint state_before_traj_msg_;
bool sampled_already_ = false;
};
/**
* \return The map between \p t1 indices ( implicitly encoded in return vector indices ) to \p t2 indices.
* If \p t1 is <tt>"{C, B}"</tt> and \p t2 is <tt>"{A, B, C, D}"</tt>, the associated mapping vector is
* <tt>"{2, 1}"</tt>.
*/
template <class T>
inline std::vector<size_t> mapping( const T & t1, const T & t2 )
{
// t1 must be a subset of t2
if ( t1.size( ) > t2.size( ) )
{
return std::vector<size_t>( );
}
std::vector<size_t> mapping_vector( t1.size( ) ); // Return value
for ( auto t1_it = t1.begin( ); t1_it != t1.end( ); ++t1_it )
{
auto t2_it = std::find( t2.begin( ), t2.end( ), *t1_it );
if ( t2.end( ) == t2_it )
{
return std::vector<size_t>( );
}
else
{
const size_t t1_dist = std::distance( t1.begin( ), t1_it );
const size_t t2_dist = std::distance( t2.begin( ), t2_it );
mapping_vector[t1_dist] = t2_dist;
}
}
return mapping_vector;
}
} // namespace joint_trajectory_controller
#endif // JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_
1 // Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/* This header must be included by all rclcpp headers which declare symbols
* which are defined in the rclcpp library. When not building the rclcpp
* library, i.e. when using the headers in other package's code, the contents
* of this header change the visibility of certain symbols which the rclcpp
* library cannot have, but the consuming code must have inorder to link.
*/
#ifndef JOINT_TRAJECTORY_CONTROLLER__VISIBILITY_CONTROL_H_
#define JOINT_TRAJECTORY_CONTROLLER__VISIBILITY_CONTROL_H_
// This logic was borrowed ( then namespaced ) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define JOINT_TRAJECTORY_CONTROLLER_EXPORT __attribute__( ( dllexport ) )
#define JOINT_TRAJECTORY_CONTROLLER_IMPORT __attribute__( ( dllimport ) )
#else
#define JOINT_TRAJECTORY_CONTROLLER_EXPORT __declspec( dllexport )
#define JOINT_TRAJECTORY_CONTROLLER_IMPORT __declspec( dllimport )
#endif
#ifdef JOINT_TRAJECTORY_CONTROLLER_BUILDING_DLL
#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC JOINT_TRAJECTORY_CONTROLLER_EXPORT
#else
#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC JOINT_TRAJECTORY_CONTROLLER_IMPORT
#endif
#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC_TYPE JOINT_TRAJECTORY_CONTROLLER_PUBLIC
#define JOINT_TRAJECTORY_CONTROLLER_LOCAL
#else
#define JOINT_TRAJECTORY_CONTROLLER_EXPORT __attribute__( ( visibility( "default" ) ) )
#define JOINT_TRAJECTORY_CONTROLLER_IMPORT
#if __GNUC__ >= 4
#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC __attribute__( ( visibility( "default" ) ) )
#define JOINT_TRAJECTORY_CONTROLLER_LOCAL __attribute__( ( visibility( "hidden" ) ) )
#else
#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC
#define JOINT_TRAJECTORY_CONTROLLER_LOCAL
#endif
#define JOINT_TRAJECTORY_CONTROLLER_PUBLIC_TYPE
#endif
#endif // JOINT_TRAJECTORY_CONTROLLER__VISIBILITY_CONTROL_H_
1 // Copyright ( c ) 2021 ros2_control Development Team
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "joint_trajectory_controller/joint_trajectory_controller.hpp"
#include <stddef.h>
#include <chrono>
#include <functional>
#include <memory>
#include <ostream>
#include <ratio>
#include <string>
#include <vector>
#include "angles/angles.h"
#include "builtin_interfaces/msg/duration.hpp"
#include "builtin_interfaces/msg/time.hpp"
#include "controller_interface/helpers.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "joint_trajectory_controller/trajectory.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/parameter.hpp"
#include "rclcpp/qos.hpp"
#include "rclcpp/qos_event.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp_action/create_server.hpp"
#include "rclcpp_action/server_goal_handle.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "std_msgs/msg/header.hpp"
namespace joint_trajectory_controller
{
48 JointTrajectoryController::JointTrajectoryController( )
: controller_interface::ControllerInterface( ), joint_names_( {} ), dof_( 0 )
{
}
53 controller_interface::CallbackReturn JointTrajectoryController::on_init( )
{
try
{
// with the lifecycle node being initialized, we can declare parameters
joint_names_ = auto_declare<std::vector<std::string>>( "joints", joint_names_ );
command_interface_types_ =
auto_declare<std::vector<std::string>>( "command_interfaces", command_interface_types_ );
state_interface_types_ =
auto_declare<std::vector<std::string>>( "state_interfaces", state_interface_types_ );
allow_partial_joints_goal_ =
auto_declare<bool>( "allow_partial_joints_goal", allow_partial_joints_goal_ );
open_loop_control_ = auto_declare<bool>( "open_loop_control", open_loop_control_ );
allow_integration_in_goal_trajectories_ = auto_declare<bool>(
"allow_integration_in_goal_trajectories", allow_integration_in_goal_trajectories_ );
state_publish_rate_ = auto_declare<double>( "state_publish_rate", 50.0 );
action_monitor_rate_ = auto_declare<double>( "action_monitor_rate", 20.0 );
std::string interpolation_string = auto_declare<std::string>(
"interpolation_method", interpolation_methods::InterpolationMethodMap.at(
interpolation_methods::DEFAULT_INTERPOLATION ) );
interpolation_method_ = interpolation_methods::from_string( interpolation_string );
}
catch ( const std::exception & e )
{
fprintf( stderr, "Exception thrown during init stage with message: %s \n", e.what( ) );
return CallbackReturn::ERROR;
}
return CallbackReturn::SUCCESS;
}
controller_interface::InterfaceConfiguration
86 JointTrajectoryController::command_interface_configuration( ) const
{
controller_interface::InterfaceConfiguration conf;
conf.type = controller_interface::interface_configuration_type::INDIVIDUAL;
if ( dof_ == 0 )
{
fprintf(
stderr,
"During ros2_control interface configuration, degrees of freedom is not valid;"
" it should be positive. Actual DOF is %zu\n",
dof_ );
std::exit( EXIT_FAILURE );
}
conf.names.reserve( dof_ * command_interface_types_.size( ) );
for ( const auto & joint_name : joint_names_ )
{
for ( const auto & interface_type : command_interface_types_ )
{
conf.names.push_back( joint_name + "/" + interface_type );
}
}
return conf;
}
controller_interface::InterfaceConfiguration
111 JointTrajectoryController::state_interface_configuration( ) const
{
controller_interface::InterfaceConfiguration conf;
conf.type = controller_interface::interface_configuration_type::INDIVIDUAL;
conf.names.reserve( dof_ * state_interface_types_.size( ) );
for ( const auto & joint_name : joint_names_ )
{
for ( const auto & interface_type : state_interface_types_ )
{
conf.names.push_back( joint_name + "/" + interface_type );
}
}
return conf;
}
126 controller_interface::return_type JointTrajectoryController::update(
127 const rclcpp::Time & time, const rclcpp::Duration & period )
{
if ( get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE )
{
return controller_interface::return_type::OK;
}
auto compute_error_for_joint = [&](
JointTrajectoryPoint & error, int index,
const JointTrajectoryPoint & current,
const JointTrajectoryPoint & desired ) {
// error defined as the difference between current and desired
error.positions[index] =
angles::shortest_angular_distance( current.positions[index], desired.positions[index] );
if ( has_velocity_state_interface_ && has_velocity_command_interface_ )
{
error.velocities[index] = desired.velocities[index] - current.velocities[index];
}
if ( has_acceleration_state_interface_ && has_acceleration_command_interface_ )
{
error.accelerations[index] = desired.accelerations[index] - current.accelerations[index];
}
};
// Check if a new external message has been received from nonRT threads
auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg( );
auto new_external_msg = traj_msg_external_point_ptr_.readFromRT( );
if ( current_external_msg != *new_external_msg )
{
fill_partial_goal( *new_external_msg );
sort_to_local_joint_order( *new_external_msg );
// TODO( denis ): Add here integration of position and velocity
traj_external_point_ptr_->update( *new_external_msg );
}
// TODO( anyone ): can I here also use const on joint_interface since the reference_wrapper is not
// changed, but its value only?
auto assign_interface_from_point =
[&]( auto & joint_interface, const std::vector<double> & trajectory_point_interface ) {
for ( size_t index = 0; index < dof_; ++index )
{
joint_interface[index].get( ).set_value( trajectory_point_interface[index] );
}
};
// current state update
state_current_.time_from_start.set__sec( 0 );
read_state_from_hardware( state_current_ );
// currently carrying out a trajectory
if ( traj_point_active_ptr_ && ( *traj_point_active_ptr_ )->has_trajectory_msg( ) )
{
bool first_sample = false;
// if sampling the first time, set the point before you sample
if ( !( *traj_point_active_ptr_ )->is_sampled_already( ) )
{
first_sample = true;
if ( open_loop_control_ )
{
( *traj_point_active_ptr_ )->set_point_before_trajectory_msg( time, last_commanded_state_ );
}
else
{
( *traj_point_active_ptr_ )->set_point_before_trajectory_msg( time, state_current_ );
}
}
// find segment for current timestamp
TrajectoryPointConstIter start_segment_itr, end_segment_itr;
const bool valid_point =
( *traj_point_active_ptr_ )
->sample( time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr );
if ( valid_point )
{
bool tolerance_violated_while_moving = false;
bool outside_goal_tolerance = false;
bool within_goal_time = true;
double time_difference = 0.0;
const bool before_last_point = end_segment_itr != ( *traj_point_active_ptr_ )->end( );
// Check state/goal tolerance
for ( size_t index = 0; index < dof_; ++index )
{
compute_error_for_joint( state_error_, index, state_current_, state_desired_ );
// Always check the state tolerance on the first sample in case the first sample
// is the last point
if (
( before_last_point || first_sample ) &&
!check_state_tolerance_per_joint(
state_error_, index, default_tolerances_.state_tolerance[index], false ) )
{
tolerance_violated_while_moving = true;
}
// past the final point, check that we end up inside goal tolerance
if (
!before_last_point &&
!check_state_tolerance_per_joint(
state_error_, index, default_tolerances_.goal_state_tolerance[index], false ) )
{
outside_goal_tolerance = true;
if ( default_tolerances_.goal_time_tolerance != 0.0 )
{
// if we exceed goal_time_tolerance set it to aborted
const rclcpp::Time traj_start = ( *traj_point_active_ptr_ )->get_trajectory_start_time( );
const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start;
time_difference = get_node( )->now( ).seconds( ) - traj_end.seconds( );
if ( time_difference > default_tolerances_.goal_time_tolerance )
{
within_goal_time = false;
}
}
}
}
// set values for next hardware write( ) if tolerance is met
if ( !tolerance_violated_while_moving && within_goal_time )
{
if ( use_closed_loop_pid_adapter_ )
{
// Update PIDs
for ( auto i = 0ul; i < dof_; ++i )
{
tmp_command_[i] = ( state_desired_.velocities[i] * ff_velocity_scale_[i] ) +
pids_[i]->computeCommand(
state_desired_.positions[i] - state_current_.positions[i],
state_desired_.velocities[i] - state_current_.velocities[i],
( uint64_t )period.nanoseconds( ) );
}
}
// set values for next hardware write( )
if ( has_position_command_interface_ )
{
assign_interface_from_point( joint_command_interface_[0], state_desired_.positions );
}
if ( has_velocity_command_interface_ )
{
if ( use_closed_loop_pid_adapter_ )
{
assign_interface_from_point( joint_command_interface_[1], tmp_command_ );
}
else
{
assign_interface_from_point( joint_command_interface_[1], state_desired_.velocities );
}
}
if ( has_acceleration_command_interface_ )
{
assign_interface_from_point( joint_command_interface_[2], state_desired_.accelerations );
}
if ( has_effort_command_interface_ )
{
if ( use_closed_loop_pid_adapter_ )
{
assign_interface_from_point( joint_command_interface_[3], tmp_command_ );
}
else
{
assign_interface_from_point( joint_command_interface_[3], state_desired_.effort );
}
}
// store the previous command. Used in open-loop control mode
last_commanded_state_ = state_desired_;
}
const auto active_goal = *rt_active_goal_.readFromRT( );
if ( active_goal )
{
// send feedback
auto feedback = std::make_shared<FollowJTrajAction::Feedback>( );
feedback->header.stamp = time;
feedback->joint_names = joint_names_;
feedback->actual = state_current_;
feedback->desired = state_desired_;
feedback->error = state_error_;
active_goal->setFeedback( feedback );
// check abort
if ( tolerance_violated_while_moving )
{
set_hold_position( );
auto result = std::make_shared<FollowJTrajAction::Result>( );
RCLCPP_WARN( get_node( )->get_logger( ), "Aborted due to state tolerance violation" );
result->set__error_code( FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED );
active_goal->setAborted( result );
// TODO( matthew-reynolds ): Need a lock-free write here
// See https://github.com/ros-controls/ros2_controllers/issues/168
rt_active_goal_.writeFromNonRT( RealtimeGoalHandlePtr( ) );
// check goal tolerance
}
else if ( !before_last_point )
{
if ( !outside_goal_tolerance )
{
auto res = std::make_shared<FollowJTrajAction::Result>( );
res->set__error_code( FollowJTrajAction::Result::SUCCESSFUL );
active_goal->setSucceeded( res );
// TODO( matthew-reynolds ): Need a lock-free write here
// See https://github.com/ros-controls/ros2_controllers/issues/168
rt_active_goal_.writeFromNonRT( RealtimeGoalHandlePtr( ) );
RCLCPP_INFO( get_node( )->get_logger( ), "Goal reached, success!" );
}
else if ( !within_goal_time )
{
set_hold_position( );
auto result = std::make_shared<FollowJTrajAction::Result>( );
result->set__error_code( FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED );
active_goal->setAborted( result );
// TODO( matthew-reynolds ): Need a lock-free write here
// See https://github.com/ros-controls/ros2_controllers/issues/168
rt_active_goal_.writeFromNonRT( RealtimeGoalHandlePtr( ) );
RCLCPP_WARN(
get_node( )->get_logger( ), "Aborted due goal_time_tolerance exceeding by %f seconds",
time_difference );
}
// else, run another cycle while waiting for outside_goal_tolerance
// to be satisfied or violated within the goal_time_tolerance
}
}
}
}
publish_state( state_desired_, state_current_, state_error_ );
return controller_interface::return_type::OK;
}
363 void JointTrajectoryController::read_state_from_hardware( JointTrajectoryPoint & state )
{
auto assign_point_from_interface =
[&]( std::vector<double> & trajectory_point_interface, const auto & joint_interface ) {
for ( size_t index = 0; index < dof_; ++index )
{
trajectory_point_interface[index] = joint_interface[index].get( ).get_value( );
}
};
// Assign values from the hardware
// Position states always exist
assign_point_from_interface( state.positions, joint_state_interface_[0] );
// velocity and acceleration states are optional
if ( has_velocity_state_interface_ )
{
assign_point_from_interface( state.velocities, joint_state_interface_[1] );
// Acceleration is used only in combination with velocity
if ( has_acceleration_state_interface_ )
{
assign_point_from_interface( state.accelerations, joint_state_interface_[2] );
}
else
{
// Make empty so the property is ignored during interpolation
state.accelerations.clear( );
}
}
else
{
// Make empty so the property is ignored during interpolation
state.velocities.clear( );
state.accelerations.clear( );
}
}
399 bool JointTrajectoryController::read_state_from_command_interfaces( JointTrajectoryPoint & state )
{
bool has_values = true;
auto assign_point_from_interface =
[&]( std::vector<double> & trajectory_point_interface, const auto & joint_interface ) {
for ( size_t index = 0; index < dof_; ++index )
{
trajectory_point_interface[index] = joint_interface[index].get( ).get_value( );
}
};
auto interface_has_values = []( const auto & joint_interface ) {
return std::find_if( joint_interface.begin( ), joint_interface.end( ), []( const auto & interface ) {
return std::isnan( interface.get( ).get_value( ) );
} ) == joint_interface.end( );
};
// Assign values from the command interfaces as state. Therefore needs check for both.
// Position state interface has to exist always
if ( has_position_command_interface_ && interface_has_values( joint_command_interface_[0] ) )
{
assign_point_from_interface( state.positions, joint_command_interface_[0] );
}
else
{
state.positions.clear( );
has_values = false;
}
// velocity and acceleration states are optional
if ( has_velocity_state_interface_ )
{
if ( has_velocity_command_interface_ && interface_has_values( joint_command_interface_[1] ) )
{
assign_point_from_interface( state.velocities, joint_command_interface_[1] );
}
else
{
state.velocities.clear( );
has_values = false;
}
}
else
{
state.velocities.clear( );
}
// Acceleration is used only in combination with velocity
if ( has_acceleration_state_interface_ )
{
if ( has_acceleration_command_interface_ && interface_has_values( joint_command_interface_[2] ) )
{
assign_point_from_interface( state.accelerations, joint_command_interface_[2] );
}
else
{
state.accelerations.clear( );
has_values = false;
}
}
else
{
state.accelerations.clear( );
}
return has_values;
}
466 controller_interface::CallbackReturn JointTrajectoryController::on_configure(
467 const rclcpp_lifecycle::State & )
{
const auto logger = get_node( )->get_logger( );
// update parameters
joint_names_ = get_node( )->get_parameter( "joints" ).as_string_array( );
if ( ( dof_ > 0 ) && ( joint_names_.size( ) != dof_ ) )
{
RCLCPP_ERROR(
logger,
"The JointTrajectoryController does not support restarting with a different number of DOF" );
// TODO( andyz ): update vector lengths if num. joints did change and re-initialize them so we
// can continue
return CallbackReturn::FAILURE;
}
dof_ = joint_names_.size( );
if ( !reset( ) )
{
return CallbackReturn::FAILURE;
}
if ( joint_names_.empty( ) )
{
RCLCPP_WARN( logger, "'joints' parameter is empty." );
}
// Specialized, child controllers set interfaces before calling configure function.
if ( command_interface_types_.empty( ) )
{
command_interface_types_ = get_node( )->get_parameter( "command_interfaces" ).as_string_array( );
}
if ( command_interface_types_.empty( ) )
{
RCLCPP_ERROR( logger, "'command_interfaces' parameter is empty." );
return CallbackReturn::FAILURE;
}
// Check if only allowed interface types are used and initialize storage to avoid memory
// allocation during activation
joint_command_interface_.resize( allowed_interface_types_.size( ) );
for ( const auto & interface : command_interface_types_ )
{
auto it =
std::find( allowed_interface_types_.begin( ), allowed_interface_types_.end( ), interface );
if ( it == allowed_interface_types_.end( ) )
{
RCLCPP_ERROR( logger, "Command interface type '%s' not allowed!", interface.c_str( ) );
return CallbackReturn::FAILURE;
}
}
// Check if command interfaces combination is valid. Valid combinations are:
// 1. effort
// 2. velocity
// 2. position [velocity, [acceleration]]
has_position_command_interface_ =
contains_interface_type( command_interface_types_, hardware_interface::HW_IF_POSITION );
has_velocity_command_interface_ =
contains_interface_type( command_interface_types_, hardware_interface::HW_IF_VELOCITY );
has_acceleration_command_interface_ =
contains_interface_type( command_interface_types_, hardware_interface::HW_IF_ACCELERATION );
has_effort_command_interface_ =
contains_interface_type( command_interface_types_, hardware_interface::HW_IF_EFFORT );
if ( has_velocity_command_interface_ )
{
// if there is only velocity then use also PID adapter
if ( command_interface_types_.size( ) == 1 )
{
use_closed_loop_pid_adapter_ = true;
}
else if ( !has_position_command_interface_ )
{
RCLCPP_ERROR(
logger,
"'velocity' command interface can be used either alone or 'position' "
"interface has to be present." );
return CallbackReturn::FAILURE;
}
// invalid: acceleration is defined and no velocity
}
else if ( has_acceleration_command_interface_ )
{
RCLCPP_ERROR(
logger,
"'acceleration' command interface can only be used if 'velocity' and "
"'position' interfaces are present" );
return CallbackReturn::FAILURE;
}
// effort can't be combined with other interfaces
if ( has_effort_command_interface_ )
{
if ( command_interface_types_.size( ) == 1 )
{
use_closed_loop_pid_adapter_ = true;
}
else
{
RCLCPP_ERROR( logger, "'effort' command interface has to be used alone." );
return CallbackReturn::FAILURE;
}
}
if ( use_closed_loop_pid_adapter_ )
{
pids_.resize( dof_ );
ff_velocity_scale_.resize( dof_ );
tmp_command_.resize( dof_, 0.0 );
// Init PID gains from ROS parameter server
for ( size_t i = 0; i < pids_.size( ); ++i )
{
const std::string prefix = "gains." + joint_names_[i];
const auto k_p = auto_declare<double>( prefix + ".p", 0.0 );
const auto k_i = auto_declare<double>( prefix + ".i", 0.0 );
const auto k_d = auto_declare<double>( prefix + ".d", 0.0 );
const auto i_clamp = auto_declare<double>( prefix + ".i_clamp", 0.0 );
ff_velocity_scale_[i] = auto_declare<double>( "ff_velocity_scale/" + joint_names_[i], 0.0 );
// Initialize PID
pids_[i] = std::make_shared<control_toolbox::Pid>( k_p, k_i, k_d, i_clamp, -i_clamp );
}
}
// Read always state interfaces from the parameter because they can be used
// independently from the controller's type.
// Specialized, child controllers should set its default value.
state_interface_types_ = get_node( )->get_parameter( "state_interfaces" ).as_string_array( );
if ( state_interface_types_.empty( ) )
{
RCLCPP_ERROR( logger, "'state_interfaces' parameter is empty." );
return CallbackReturn::FAILURE;
}
if ( contains_interface_type( state_interface_types_, hardware_interface::HW_IF_EFFORT ) )
{
RCLCPP_ERROR( logger, "State interface type 'effort' not allowed!" );
return CallbackReturn::FAILURE;
}
// Check if only allowed interface types are used and initialize storage to avoid memory
// allocation during activation
// Note: 'effort' storage is also here, but never used. Still, for this is OK.
joint_state_interface_.resize( allowed_interface_types_.size( ) );
for ( const auto & interface : state_interface_types_ )
{
auto it =
std::find( allowed_interface_types_.begin( ), allowed_interface_types_.end( ), interface );
if ( it == allowed_interface_types_.end( ) )
{
RCLCPP_ERROR( logger, "State interface type '%s' not allowed!", interface.c_str( ) );
return CallbackReturn::FAILURE;
}
}
has_position_state_interface_ =
contains_interface_type( state_interface_types_, hardware_interface::HW_IF_POSITION );
has_velocity_state_interface_ =
contains_interface_type( state_interface_types_, hardware_interface::HW_IF_VELOCITY );
has_acceleration_state_interface_ =
contains_interface_type( state_interface_types_, hardware_interface::HW_IF_ACCELERATION );
if ( has_velocity_state_interface_ )
{
if ( !has_position_state_interface_ )
{
RCLCPP_ERROR(
logger,
"'velocity' state interface cannot be used if 'position' interface "
"is missing." );
return CallbackReturn::FAILURE;
}
}
else
{
if ( has_acceleration_state_interface_ )
{
RCLCPP_ERROR(
logger,
"'acceleration' state interface cannot be used if 'position' and 'velocity' "
"interfaces are not present." );
return CallbackReturn::FAILURE;
}
if ( has_velocity_command_interface_ && command_interface_types_.size( ) == 1 )
{
RCLCPP_ERROR(
logger,
"'velocity' command interface can only be used alone if 'velocity' and "
"'position' state interfaces are present" );
return CallbackReturn::FAILURE;
}
// effort is always used alone so no need for size check
if ( has_effort_command_interface_ )
{
RCLCPP_ERROR(
logger,
"'effort' command interface can only be used alone if 'velocity' and "
"'position' state interfaces are present" );
return CallbackReturn::FAILURE;
}
}
auto get_interface_list = []( const std::vector<std::string> & interface_types ) {
std::stringstream ss_interfaces;
for ( size_t index = 0; index < interface_types.size( ); ++index )
{
if ( index != 0 )
{
ss_interfaces << " ";
}
ss_interfaces << interface_types[index];
}
return ss_interfaces.str( );
};
// Print output so users can be sure the interface setup is correct
RCLCPP_INFO(
logger, "Command interfaces are [%s] and state interfaces are [%s].",
get_interface_list( command_interface_types_ ).c_str( ),
get_interface_list( state_interface_types_ ).c_str( ) );
default_tolerances_ = get_segment_tolerances( *get_node( ), joint_names_ );
// Read parameters customizing controller for special cases
open_loop_control_ = get_node( )->get_parameter( "open_loop_control" ).get_value<bool>( );
allow_integration_in_goal_trajectories_ =
get_node( )->get_parameter( "allow_integration_in_goal_trajectories" ).get_value<bool>( );
// subscriber callback
// non realtime
// TODO( karsten ): check if traj msg and point time are valid
auto callback = [this]( const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> msg ) -> void {
if ( !validate_trajectory_msg( *msg ) )
{
return;
}
// http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement
// always replace old msg with new one for now
if ( subscriber_is_active_ )
{
add_new_trajectory_msg( msg );
}
};
// TODO( karsten1987 ): create subscriber with subscription deactivated
joint_command_subscriber_ =
get_node( )->create_subscription<trajectory_msgs::msg::JointTrajectory>(
"~/joint_trajectory", rclcpp::SystemDefaultsQoS( ), callback );
// TODO( karsten1987 ): no lifecycle for subscriber yet
// joint_command_subscriber_->on_activate( );
// State publisher
RCLCPP_INFO( logger, "Controller state will be published at %.2f Hz.", state_publish_rate_ );
if ( state_publish_rate_ > 0.0 )
{
state_publisher_period_ = rclcpp::Duration::from_seconds( 1.0 / state_publish_rate_ );
}
else
{
state_publisher_period_ = rclcpp::Duration::from_seconds( 0.0 );
}
publisher_ =
get_node( )->create_publisher<ControllerStateMsg>( "~/state", rclcpp::SystemDefaultsQoS( ) );
state_publisher_ = std::make_unique<StatePublisher>( publisher_ );
state_publisher_->lock( );
state_publisher_->msg_.joint_names = joint_names_;
state_publisher_->msg_.desired.positions.resize( dof_ );
state_publisher_->msg_.desired.velocities.resize( dof_ );
state_publisher_->msg_.desired.accelerations.resize( dof_ );
state_publisher_->msg_.actual.positions.resize( dof_ );
state_publisher_->msg_.error.positions.resize( dof_ );
if ( has_velocity_state_interface_ )
{
state_publisher_->msg_.actual.velocities.resize( dof_ );
state_publisher_->msg_.error.velocities.resize( dof_ );
}
if ( has_acceleration_state_interface_ )
{
state_publisher_->msg_.actual.accelerations.resize( dof_ );
state_publisher_->msg_.error.accelerations.resize( dof_ );
}
state_publisher_->unlock( );
last_state_publish_time_ = get_node( )->now( );
// action server configuration
allow_partial_joints_goal_ =
get_node( )->get_parameter( "allow_partial_joints_goal" ).get_value<bool>( );
if ( allow_partial_joints_goal_ )
{
RCLCPP_INFO( logger, "Goals with partial set of joints are allowed" );
}
RCLCPP_INFO( logger, "Action status changes will be monitored at %.2f Hz.", action_monitor_rate_ );
action_monitor_period_ = rclcpp::Duration::from_seconds( 1.0 / action_monitor_rate_ );
using namespace std::placeholders;
action_server_ = rclcpp_action::create_server<FollowJTrajAction>(
get_node( )->get_node_base_interface( ), get_node( )->get_node_clock_interface( ),
get_node( )->get_node_logging_interface( ), get_node( )->get_node_waitables_interface( ),
std::string( get_node( )->get_name( ) ) + "/follow_joint_trajectory",
std::bind( &JointTrajectoryController::goal_callback, this, _1, _2 ),
std::bind( &JointTrajectoryController::cancel_callback, this, _1 ),
std::bind( &JointTrajectoryController::feedback_setup_callback, this, _1 ) );
resize_joint_trajectory_point( state_current_, dof_ );
resize_joint_trajectory_point( state_desired_, dof_ );
resize_joint_trajectory_point( state_error_, dof_ );
resize_joint_trajectory_point( last_commanded_state_, dof_ );
return CallbackReturn::SUCCESS;
}
789 controller_interface::CallbackReturn JointTrajectoryController::on_activate(
790 const rclcpp_lifecycle::State & )
{
// order all joints in the storage
for ( const auto & interface : command_interface_types_ )
{
auto it =
std::find( allowed_interface_types_.begin( ), allowed_interface_types_.end( ), interface );
auto index = std::distance( allowed_interface_types_.begin( ), it );
if ( !controller_interface::get_ordered_interfaces(
command_interfaces_, joint_names_, interface, joint_command_interface_[index] ) )
{
RCLCPP_ERROR(
get_node( )->get_logger( ), "Expected %zu '%s' command interfaces, got %zu.", dof_,
interface.c_str( ), joint_command_interface_[index].size( ) );
return CallbackReturn::ERROR;
}
}
for ( const auto & interface : state_interface_types_ )
{
auto it =
std::find( allowed_interface_types_.begin( ), allowed_interface_types_.end( ), interface );
auto index = std::distance( allowed_interface_types_.begin( ), it );
if ( !controller_interface::get_ordered_interfaces(
state_interfaces_, joint_names_, interface, joint_state_interface_[index] ) )
{
RCLCPP_ERROR(
get_node( )->get_logger( ), "Expected %zu '%s' state interfaces, got %zu.", dof_,
interface.c_str( ), joint_state_interface_[index].size( ) );
return CallbackReturn::ERROR;
}
}
// Store 'home' pose
traj_msg_home_ptr_ = std::make_shared<trajectory_msgs::msg::JointTrajectory>( );
traj_msg_home_ptr_->header.stamp.sec = 0;
traj_msg_home_ptr_->header.stamp.nanosec = 0;
traj_msg_home_ptr_->points.resize( 1 );
traj_msg_home_ptr_->points[0].time_from_start.sec = 0;
traj_msg_home_ptr_->points[0].time_from_start.nanosec = 50000000;
traj_msg_home_ptr_->points[0].positions.resize( joint_state_interface_[0].size( ) );
for ( size_t index = 0; index < joint_state_interface_[0].size( ); ++index )
{
traj_msg_home_ptr_->points[0].positions[index] =
joint_state_interface_[0][index].get( ).get_value( );
}
traj_external_point_ptr_ = std::make_shared<Trajectory>( );
traj_home_point_ptr_ = std::make_shared<Trajectory>( );
traj_msg_external_point_ptr_.writeFromNonRT(
std::shared_ptr<trajectory_msgs::msg::JointTrajectory>( ) );
subscriber_is_active_ = true;
traj_point_active_ptr_ = &traj_external_point_ptr_;
last_state_publish_time_ = get_node( )->now( );
// Initialize current state storage if hardware state has tracking offset
read_state_from_hardware( state_current_ );
read_state_from_hardware( state_desired_ );
read_state_from_hardware( last_commanded_state_ );
// Handle restart of controller by reading from commands if
// those are not nan
trajectory_msgs::msg::JointTrajectoryPoint state;
resize_joint_trajectory_point( state, dof_ );
if ( read_state_from_command_interfaces( state ) )
{
state_current_ = state;
state_desired_ = state;
last_commanded_state_ = state;
}
// TODO( karsten1987 ): activate subscriptions of subscriber
return CallbackReturn::SUCCESS;
}
864 controller_interface::CallbackReturn JointTrajectoryController::on_deactivate(
865 const rclcpp_lifecycle::State & )
{
// TODO( anyone ): How to halt when using effort commands?
for ( size_t index = 0; index < dof_; ++index )
{
if ( has_position_command_interface_ )
{
joint_command_interface_[0][index].get( ).set_value(
joint_command_interface_[0][index].get( ).get_value( ) );
}
if ( has_velocity_command_interface_ )
{
joint_command_interface_[1][index].get( ).set_value( 0.0 );
}
if ( has_effort_command_interface_ )
{
joint_command_interface_[3][index].get( ).set_value( 0.0 );
}
}
for ( size_t index = 0; index < allowed_interface_types_.size( ); ++index )
{
joint_command_interface_[index].clear( );
joint_state_interface_[index].clear( );
}
release_interfaces( );
subscriber_is_active_ = false;
return CallbackReturn::SUCCESS;
}
899 controller_interface::CallbackReturn JointTrajectoryController::on_cleanup(
900 const rclcpp_lifecycle::State & )
{
// go home
traj_home_point_ptr_->update( traj_msg_home_ptr_ );
traj_point_active_ptr_ = &traj_home_point_ptr_;
return CallbackReturn::SUCCESS;
}
909 controller_interface::CallbackReturn JointTrajectoryController::on_error(
910 const rclcpp_lifecycle::State & )
{
if ( !reset( ) )
{
return CallbackReturn::ERROR;
}
return CallbackReturn::SUCCESS;
}
919 bool JointTrajectoryController::reset( )
{
subscriber_is_active_ = false;
joint_command_subscriber_.reset( );
for ( const auto & pid : pids_ )
{
pid->reset( );
}
// iterator has no default value
// prev_traj_point_ptr_;
traj_point_active_ptr_ = nullptr;
traj_external_point_ptr_.reset( );
traj_home_point_ptr_.reset( );
traj_msg_home_ptr_.reset( );
// reset pids
for ( const auto & pid : pids_ )
{
pid->reset( );
}
return true;
}
945 controller_interface::CallbackReturn JointTrajectoryController::on_shutdown(
946 const rclcpp_lifecycle::State & )
{
// TODO( karsten1987 ): what to do?
return CallbackReturn::SUCCESS;
}
953 void JointTrajectoryController::publish_state(
954 const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state,
955 const JointTrajectoryPoint & state_error )
{
if ( state_publisher_period_.seconds( ) <= 0.0 )
{
return;
}
if ( get_node( )->now( ) < ( last_state_publish_time_ + state_publisher_period_ ) )
{
return;
}
if ( state_publisher_ && state_publisher_->trylock( ) )
{
last_state_publish_time_ = get_node( )->now( );
state_publisher_->msg_.header.stamp = last_state_publish_time_;
state_publisher_->msg_.desired.positions = desired_state.positions;
state_publisher_->msg_.desired.velocities = desired_state.velocities;
state_publisher_->msg_.desired.accelerations = desired_state.accelerations;
state_publisher_->msg_.actual.positions = current_state.positions;
state_publisher_->msg_.error.positions = state_error.positions;
if ( has_velocity_state_interface_ )
{
state_publisher_->msg_.actual.velocities = current_state.velocities;
state_publisher_->msg_.error.velocities = state_error.velocities;
}
if ( has_acceleration_state_interface_ )
{
state_publisher_->msg_.actual.accelerations = current_state.accelerations;
state_publisher_->msg_.error.accelerations = state_error.accelerations;
}
state_publisher_->unlockAndPublish( );
}
}
991 rclcpp_action::GoalResponse JointTrajectoryController::goal_callback(
992 const rclcpp_action::GoalUUID &, std::shared_ptr<const FollowJTrajAction::Goal> goal )
{
RCLCPP_INFO( get_node( )->get_logger( ), "Received new action goal" );
// Precondition: Running controller
if ( get_state( ).id( ) == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE )
{
RCLCPP_ERROR(
get_node( )->get_logger( ), "Can't accept new action goals. Controller is not running." );
return rclcpp_action::GoalResponse::REJECT;
}
if ( !validate_trajectory_msg( goal->trajectory ) )
{
return rclcpp_action::GoalResponse::REJECT;
}
RCLCPP_INFO( get_node( )->get_logger( ), "Accepted new action goal" );
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
1013 rclcpp_action::CancelResponse JointTrajectoryController::cancel_callback(
const std::shared_ptr<rclcpp_action::ServerGoalHandle<FollowJTrajAction>> goal_handle )
{
RCLCPP_INFO( get_node( )->get_logger( ), "Got request to cancel goal" );
// Check that cancel request refers to currently active goal ( if any )
const auto active_goal = *rt_active_goal_.readFromNonRT( );
if ( active_goal && active_goal->gh_ == goal_handle )
{
// Controller uptime
// Enter hold current position mode
set_hold_position( );
RCLCPP_DEBUG(
get_node( )->get_logger( ), "Canceling active action goal because cancel callback received." );
// Mark the current goal as canceled
auto action_res = std::make_shared<FollowJTrajAction::Result>( );
active_goal->setCanceled( action_res );
rt_active_goal_.writeFromNonRT( RealtimeGoalHandlePtr( ) );
}
return rclcpp_action::CancelResponse::ACCEPT;
}
1037 void JointTrajectoryController::feedback_setup_callback(
std::shared_ptr<rclcpp_action::ServerGoalHandle<FollowJTrajAction>> goal_handle )
{
// Update new trajectory
{
preempt_active_goal( );
auto traj_msg =
std::make_shared<trajectory_msgs::msg::JointTrajectory>( goal_handle->get_goal( )->trajectory );
add_new_trajectory_msg( traj_msg );
}
// Update the active goal
RealtimeGoalHandlePtr rt_goal = std::make_shared<RealtimeGoalHandle>( goal_handle );
rt_goal->preallocated_feedback_->joint_names = joint_names_;
rt_goal->execute( );
rt_active_goal_.writeFromNonRT( rt_goal );
// Setup goal status checking timer
goal_handle_timer_ = get_node( )->create_wall_timer(
action_monitor_period_.to_chrono<std::chrono::seconds>( ),
std::bind( &RealtimeGoalHandle::runNonRealtime, rt_goal ) );
}
1061 void JointTrajectoryController::fill_partial_goal(
1062 std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg ) const
{
// joint names in the goal are a subset of existing joints, as checked in goal_callback
// so if the size matches, the goal contains all controller joints
if ( dof_ == trajectory_msg->joint_names.size( ) )
{
return;
}
trajectory_msg->joint_names.reserve( dof_ );
for ( size_t index = 0; index < dof_; ++index )
{
{
if (
std::find(
trajectory_msg->joint_names.begin( ), trajectory_msg->joint_names.end( ),
joint_names_[index] ) != trajectory_msg->joint_names.end( ) )
{
// joint found on msg
continue;
}
trajectory_msg->joint_names.push_back( joint_names_[index] );
for ( auto & it : trajectory_msg->points )
{
// Assume hold position with 0 velocity and acceleration for missing joints
if ( !it.positions.empty( ) )
{
if (
has_position_command_interface_ &&
!std::isnan( joint_command_interface_[0][index].get( ).get_value( ) ) )
{
// copy last command if cmd interface exists
it.positions.push_back( joint_command_interface_[0][index].get( ).get_value( ) );
}
else if ( has_position_state_interface_ )
{
// copy current state if state interface exists
it.positions.push_back( joint_state_interface_[0][index].get( ).get_value( ) );
}
}
if ( !it.velocities.empty( ) )
{
it.velocities.push_back( 0.0 );
}
if ( !it.accelerations.empty( ) )
{
it.accelerations.push_back( 0.0 );
}
if ( !it.effort.empty( ) )
{
it.effort.push_back( 0.0 );
}
}
}
}
}
1121 void JointTrajectoryController::sort_to_local_joint_order(
1122 std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg )
{
// rearrange all points in the trajectory message based on mapping
std::vector<size_t> mapping_vector = mapping( trajectory_msg->joint_names, joint_names_ );
auto remap = [this](
const std::vector<double> & to_remap,
const std::vector<size_t> & mapping ) -> std::vector<double> {
if ( to_remap.empty( ) )
{
return to_remap;
}
if ( to_remap.size( ) != mapping.size( ) )
{
RCLCPP_WARN(
get_node( )->get_logger( ), "Invalid input size ( %zu ) for sorting", to_remap.size( ) );
return to_remap;
}
std::vector<double> output;
output.resize( mapping.size( ), 0.0 );
for ( size_t index = 0; index < mapping.size( ); ++index )
{
auto map_index = mapping[index];
output[map_index] = to_remap[index];
}
return output;
};
for ( size_t index = 0; index < trajectory_msg->points.size( ); ++index )
{
trajectory_msg->points[index].positions =
remap( trajectory_msg->points[index].positions, mapping_vector );
trajectory_msg->points[index].velocities =
remap( trajectory_msg->points[index].velocities, mapping_vector );
trajectory_msg->points[index].accelerations =
remap( trajectory_msg->points[index].accelerations, mapping_vector );
trajectory_msg->points[index].effort =
remap( trajectory_msg->points[index].effort, mapping_vector );
}
}
1165 bool JointTrajectoryController::validate_trajectory_point_field(
1166 size_t joint_names_size, const std::vector<double> & vector_field,
1167 const std::string & string_for_vector_field, size_t i, bool allow_empty ) const
{
if ( allow_empty && vector_field.empty( ) )
{
return true;
}
if ( joint_names_size != vector_field.size( ) )
{
RCLCPP_ERROR(
get_node( )->get_logger( ), "Mismatch between joint_names ( %zu ) and %s ( %zu ) at point #%zu.",
joint_names_size, string_for_vector_field.c_str( ), vector_field.size( ), i );
return false;
}
return true;
}
1183 bool JointTrajectoryController::validate_trajectory_msg(
1184 const trajectory_msgs::msg::JointTrajectory & trajectory ) const
{
// If partial joints goals are not allowed, goal should specify all controller joints
if ( !allow_partial_joints_goal_ )
{
if ( trajectory.joint_names.size( ) != dof_ )
{
RCLCPP_ERROR(
get_node( )->get_logger( ),
"Joints on incoming trajectory don't match the controller joints." );
return false;
}
}
if ( trajectory.joint_names.empty( ) )
{
RCLCPP_ERROR( get_node( )->get_logger( ), "Empty joint names on incoming trajectory." );
return false;
}
const auto trajectory_start_time = static_cast<rclcpp::Time>( trajectory.header.stamp );
// If the starting time it set to 0.0, it means the controller should start it now.
// Otherwise we check if the trajectory ends before the current time,
// in which case it can be ignored.
if ( trajectory_start_time.seconds( ) != 0.0 )
{
auto trajectory_end_time = trajectory_start_time;
for ( const auto & p : trajectory.points )
{
trajectory_end_time += p.time_from_start;
}
if ( trajectory_end_time < get_node( )->now( ) )
{
RCLCPP_ERROR(
get_node( )->get_logger( ),
"Received trajectory with non zero time start time ( %f ) that ends on the past ( %f )",
trajectory_start_time.seconds( ), trajectory_end_time.seconds( ) );
return false;
}
}
for ( size_t i = 0; i < trajectory.joint_names.size( ); ++i )
{
const std::string & incoming_joint_name = trajectory.joint_names[i];
auto it = std::find( joint_names_.begin( ), joint_names_.end( ), incoming_joint_name );
if ( it == joint_names_.end( ) )
{
RCLCPP_ERROR(
get_node( )->get_logger( ), "Incoming joint %s doesn't match the controller's joints.",
incoming_joint_name.c_str( ) );
return false;
}
}
rclcpp::Duration previous_traj_time( 0ms );
for ( size_t i = 0; i < trajectory.points.size( ); ++i )
{
if ( ( i > 0 ) && ( rclcpp::Duration( trajectory.points[i].time_from_start ) <= previous_traj_time ) )
{
RCLCPP_ERROR(
get_node( )->get_logger( ),
"Time between points %zu and %zu is not strictly increasing, it is %f and %f respectively",
i - 1, i, previous_traj_time.seconds( ),
rclcpp::Duration( trajectory.points[i].time_from_start ).seconds( ) );
return false;
}
previous_traj_time = trajectory.points[i].time_from_start;
const size_t joint_count = trajectory.joint_names.size( );
const auto & points = trajectory.points;
// This currently supports only position, velocity and acceleration inputs
if ( allow_integration_in_goal_trajectories_ )
{
const bool all_empty = points[i].positions.empty( ) && points[i].velocities.empty( ) &&
points[i].accelerations.empty( );
const bool position_error =
!points[i].positions.empty( ) &&
!validate_trajectory_point_field( joint_count, points[i].positions, "positions", i, false );
const bool velocity_error =
!points[i].velocities.empty( ) &&
!validate_trajectory_point_field( joint_count, points[i].velocities, "velocities", i, false );
const bool acceleration_error =
!points[i].accelerations.empty( ) &&
!validate_trajectory_point_field(
joint_count, points[i].accelerations, "accelerations", i, false );
if ( all_empty || position_error || velocity_error || acceleration_error )
{
return false;
}
}
else if (
!validate_trajectory_point_field( joint_count, points[i].positions, "positions", i, false ) ||
!validate_trajectory_point_field( joint_count, points[i].velocities, "velocities", i, true ) ||
!validate_trajectory_point_field(
joint_count, points[i].accelerations, "accelerations", i, true ) ||
!validate_trajectory_point_field( joint_count, points[i].effort, "effort", i, true ) )
{
return false;
}
}
return true;
}
1288 void JointTrajectoryController::add_new_trajectory_msg(
1289 const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg )
{
traj_msg_external_point_ptr_.writeFromNonRT( traj_msg );
}
1294 void JointTrajectoryController::preempt_active_goal( )
{
const auto active_goal = *rt_active_goal_.readFromNonRT( );
if ( active_goal )
{
set_hold_position( );
auto action_res = std::make_shared<FollowJTrajAction::Result>( );
action_res->set__error_code( FollowJTrajAction::Result::INVALID_GOAL );
action_res->set__error_string( "Current goal cancelled due to new incoming action." );
active_goal->setCanceled( action_res );
rt_active_goal_.writeFromNonRT( RealtimeGoalHandlePtr( ) );
}
}
1308 void JointTrajectoryController::set_hold_position( )
{
trajectory_msgs::msg::JointTrajectory empty_msg;
empty_msg.header.stamp = rclcpp::Time( 0 );
auto traj_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>( empty_msg );
add_new_trajectory_msg( traj_msg );
}
1317 bool JointTrajectoryController::contains_interface_type(
1318 const std::vector<std::string> & interface_type_list, const std::string & interface_type )
{
return std::find( interface_type_list.begin( ), interface_type_list.end( ), interface_type ) !=
interface_type_list.end( );
}
1324 void JointTrajectoryController::resize_joint_trajectory_point(
1325 trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size )
{
point.positions.resize( size, 0.0 );
if ( has_velocity_state_interface_ )
{
point.velocities.resize( size, 0.0 );
}
if ( has_acceleration_state_interface_ )
{
point.accelerations.resize( size, 0.0 );
}
}
} // namespace joint_trajectory_controller
#include "pluginlib/class_list_macros.hpp"
1342 PLUGINLIB_EXPORT_CLASS(
joint_trajectory_controller::JointTrajectoryController, controller_interface::ControllerInterface )
1 // Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "joint_trajectory_controller/trajectory.hpp"
#include <memory>
#include "hardware_interface/macros.hpp"
#include "rclcpp/duration.hpp"
#include "rclcpp/time.hpp"
#include "rcppmath/clamp.hpp"
#include "std_msgs/msg/header.hpp"
namespace joint_trajectory_controller
{
26 Trajectory::Trajectory( ) : trajectory_start_time_( 0 ), time_before_traj_msg_( 0 ) {}
28 Trajectory::Trajectory( std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory )
: trajectory_msg_( joint_trajectory ),
trajectory_start_time_( static_cast<rclcpp::Time>( joint_trajectory->header.stamp ) )
{
}
34 Trajectory::Trajectory(
35 const rclcpp::Time & current_time,
36 const trajectory_msgs::msg::JointTrajectoryPoint & current_point,
37 std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory )
: trajectory_msg_( joint_trajectory ),
trajectory_start_time_( static_cast<rclcpp::Time>( joint_trajectory->header.stamp ) )
{
set_point_before_trajectory_msg( current_time, current_point );
update( joint_trajectory );
}
45 void Trajectory::set_point_before_trajectory_msg(
46 const rclcpp::Time & current_time,
47 const trajectory_msgs::msg::JointTrajectoryPoint & current_point )
{
time_before_traj_msg_ = current_time;
state_before_traj_msg_ = current_point;
}
53 void Trajectory::update( std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory )
{
trajectory_msg_ = joint_trajectory;
trajectory_start_time_ = static_cast<rclcpp::Time>( joint_trajectory->header.stamp );
sampled_already_ = false;
}
60 bool Trajectory::sample(
61 const rclcpp::Time & sample_time,
62 const interpolation_methods::InterpolationMethod interpolation_method,
63 trajectory_msgs::msg::JointTrajectoryPoint & output_state,
64 TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr )
{
THROW_ON_NULLPTR( trajectory_msg_ )
output_state = trajectory_msgs::msg::JointTrajectoryPoint( );
if ( trajectory_msg_->points.empty( ) )
{
start_segment_itr = end( );
end_segment_itr = end( );
return false;
}
// first sampling of this trajectory
if ( !sampled_already_ )
{
if ( trajectory_start_time_.seconds( ) == 0.0 )
{
trajectory_start_time_ = sample_time;
}
sampled_already_ = true;
}
// sampling before the current point
if ( sample_time < time_before_traj_msg_ )
{
return false;
}
auto & first_point_in_msg = trajectory_msg_->points[0];
const rclcpp::Time first_point_timestamp =
trajectory_start_time_ + first_point_in_msg.time_from_start;
// current time hasn't reached traj time of the first point in the msg yet
if ( sample_time < first_point_timestamp )
{
// If interpolation is disabled, just forward the next waypoint
if ( interpolation_method == interpolation_methods::InterpolationMethod::NONE )
{
output_state = state_before_traj_msg_;
}
else
{
// it changes points only if position and velocity do not exist, but their derivatives
deduce_from_derivatives(
state_before_traj_msg_, first_point_in_msg, state_before_traj_msg_.positions.size( ),
( first_point_timestamp - time_before_traj_msg_ ).seconds( ) );
interpolate_between_points(
time_before_traj_msg_, state_before_traj_msg_, first_point_timestamp, first_point_in_msg,
sample_time, output_state );
}
start_segment_itr = begin( ); // no segments before the first
end_segment_itr = begin( );
return true;
}
// time_from_start + trajectory time is the expected arrival time of trajectory
const auto last_idx = trajectory_msg_->points.size( ) - 1;
for ( size_t i = 0; i < last_idx; ++i )
{
auto & point = trajectory_msg_->points[i];
auto & next_point = trajectory_msg_->points[i + 1];
const rclcpp::Time t0 = trajectory_start_time_ + point.time_from_start;
const rclcpp::Time t1 = trajectory_start_time_ + next_point.time_from_start;
if ( sample_time >= t0 && sample_time < t1 )
{
// If interpolation is disabled, just forward the next waypoint
if ( interpolation_method == interpolation_methods::InterpolationMethod::NONE )
{
output_state = next_point;
}
// Do interpolation
else
{
// it changes points only if position and velocity do not exist, but their derivatives
deduce_from_derivatives(
point, next_point, state_before_traj_msg_.positions.size( ), ( t1 - t0 ).seconds( ) );
interpolate_between_points( t0, point, t1, next_point, sample_time, output_state );
}
start_segment_itr = begin( ) + i;
end_segment_itr = begin( ) + ( i + 1 );
return true;
}
}
// whole animation has played out
start_segment_itr = --end( );
end_segment_itr = end( );
output_state = ( *start_segment_itr );
// the trajectories in msg may have empty velocities/accel, so resize them
if ( output_state.velocities.empty( ) )
{
output_state.velocities.resize( output_state.positions.size( ), 0.0 );
}
if ( output_state.accelerations.empty( ) )
{
output_state.accelerations.resize( output_state.positions.size( ), 0.0 );
}
return true;
}
169 void Trajectory::interpolate_between_points(
170 const rclcpp::Time & time_a, const trajectory_msgs::msg::JointTrajectoryPoint & state_a,
171 const rclcpp::Time & time_b, const trajectory_msgs::msg::JointTrajectoryPoint & state_b,
172 const rclcpp::Time & sample_time, trajectory_msgs::msg::JointTrajectoryPoint & output )
{
rclcpp::Duration duration_so_far = sample_time - time_a;
rclcpp::Duration duration_btwn_points = time_b - time_a;
const size_t dim = state_a.positions.size( );
output.positions.resize( dim, 0.0 );
output.velocities.resize( dim, 0.0 );
output.accelerations.resize( dim, 0.0 );
auto generate_powers = []( int n, double x, double * powers ) {
powers[0] = 1.0;
for ( int i = 1; i <= n; ++i )
{
powers[i] = powers[i - 1] * x;
}
};
bool has_velocity = !state_a.velocities.empty( ) && !state_b.velocities.empty( );
bool has_accel = !state_a.accelerations.empty( ) && !state_b.accelerations.empty( );
if ( duration_so_far.seconds( ) < 0.0 )
{
duration_so_far = rclcpp::Duration::from_seconds( 0.0 );
has_velocity = has_accel = false;
}
if ( duration_so_far.seconds( ) > duration_btwn_points.seconds( ) )
{
duration_so_far = duration_btwn_points;
has_velocity = has_accel = false;
}
double t[6];
generate_powers( 5, duration_so_far.seconds( ), t );
if ( !has_velocity && !has_accel )
{
// do linear interpolation
for ( size_t i = 0; i < dim; ++i )
{
double start_pos = state_a.positions[i];
double end_pos = state_b.positions[i];
double coefficients[2] = {0.0, 0.0};
coefficients[0] = start_pos;
if ( duration_btwn_points.seconds( ) != 0.0 )
{
coefficients[1] = ( end_pos - start_pos ) / duration_btwn_points.seconds( );
}
output.positions[i] = t[0] * coefficients[0] + t[1] * coefficients[1];
output.velocities[i] = t[0] * coefficients[1];
}
}
else if ( has_velocity && !has_accel )
{
// do cubic interpolation
double T[4];
generate_powers( 3, duration_btwn_points.seconds( ), T );
for ( size_t i = 0; i < dim; ++i )
{
double start_pos = state_a.positions[i];
double start_vel = state_a.velocities[i];
double end_pos = state_b.positions[i];
double end_vel = state_b.velocities[i];
double coefficients[4] = {0.0, 0.0, 0.0, 0.0};
coefficients[0] = start_pos;
coefficients[1] = start_vel;
if ( duration_btwn_points.seconds( ) != 0.0 )
{
coefficients[2] =
( -3.0 * start_pos + 3.0 * end_pos - 2.0 * start_vel * T[1] - end_vel * T[1] ) / T[2];
coefficients[3] =
( 2.0 * start_pos - 2.0 * end_pos + start_vel * T[1] + end_vel * T[1] ) / T[3];
}
output.positions[i] = t[0] * coefficients[0] + t[1] * coefficients[1] +
t[2] * coefficients[2] + t[3] * coefficients[3];
output.velocities[i] =
t[0] * coefficients[1] + t[1] * 2.0 * coefficients[2] + t[2] * 3.0 * coefficients[3];
output.accelerations[i] = t[0] * 2.0 * coefficients[2] + t[1] * 6.0 * coefficients[3];
}
}
else if ( has_velocity && has_accel )
{
// do quintic interpolation
double T[6];
generate_powers( 5, duration_btwn_points.seconds( ), T );
for ( size_t i = 0; i < dim; ++i )
{
double start_pos = state_a.positions[i];
double start_vel = state_a.velocities[i];
double start_acc = state_a.accelerations[i];
double end_pos = state_b.positions[i];
double end_vel = state_b.velocities[i];
double end_acc = state_b.accelerations[i];
double coefficients[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
coefficients[0] = start_pos;
coefficients[1] = start_vel;
coefficients[2] = 0.5 * start_acc;
if ( duration_btwn_points.seconds( ) != 0.0 )
{
coefficients[3] = ( -20.0 * start_pos + 20.0 * end_pos - 3.0 * start_acc * T[2] +
end_acc * T[2] - 12.0 * start_vel * T[1] - 8.0 * end_vel * T[1] ) /
( 2.0 * T[3] );
coefficients[4] = ( 30.0 * start_pos - 30.0 * end_pos + 3.0 * start_acc * T[2] -
2.0 * end_acc * T[2] + 16.0 * start_vel * T[1] + 14.0 * end_vel * T[1] ) /
( 2.0 * T[4] );
coefficients[5] = ( -12.0 * start_pos + 12.0 * end_pos - start_acc * T[2] + end_acc * T[2] -
6.0 * start_vel * T[1] - 6.0 * end_vel * T[1] ) /
( 2.0 * T[5] );
}
output.positions[i] = t[0] * coefficients[0] + t[1] * coefficients[1] +
t[2] * coefficients[2] + t[3] * coefficients[3] +
t[4] * coefficients[4] + t[5] * coefficients[5];
output.velocities[i] = t[0] * coefficients[1] + t[1] * 2.0 * coefficients[2] +
t[2] * 3.0 * coefficients[3] + t[3] * 4.0 * coefficients[4] +
t[4] * 5.0 * coefficients[5];
output.accelerations[i] = t[0] * 2.0 * coefficients[2] + t[1] * 6.0 * coefficients[3] +
t[2] * 12.0 * coefficients[4] + t[3] * 20.0 * coefficients[5];
}
}
}
300 void Trajectory::deduce_from_derivatives(
301 trajectory_msgs::msg::JointTrajectoryPoint & first_state,
302 trajectory_msgs::msg::JointTrajectoryPoint & second_state, const size_t dim, const double delta_t )
{
if ( second_state.positions.empty( ) )
{
second_state.positions.resize( dim );
if ( first_state.velocities.empty( ) )
{
first_state.velocities.resize( dim, 0.0 );
}
if ( second_state.velocities.empty( ) )
{
second_state.velocities.resize( dim );
if ( first_state.accelerations.empty( ) )
{
first_state.accelerations.resize( dim, 0.0 );
}
for ( size_t i = 0; i < dim; ++i )
{
second_state.velocities[i] =
first_state.velocities[i] +
( first_state.accelerations[i] + second_state.accelerations[i] ) * 0.5 * delta_t;
}
}
for ( size_t i = 0; i < dim; ++i )
{
// second state velocity should be reached on the end of the segment, so use middle
second_state.positions[i] =
first_state.positions[i] +
( first_state.velocities[i] + second_state.velocities[i] ) * 0.5 * delta_t;
}
}
}
335 TrajectoryPointConstIter Trajectory::begin( ) const
{
THROW_ON_NULLPTR( trajectory_msg_ )
return trajectory_msg_->points.begin( );
}
342 TrajectoryPointConstIter Trajectory::end( ) const
{
THROW_ON_NULLPTR( trajectory_msg_ )
return trajectory_msg_->points.end( );
}
349 rclcpp::Time Trajectory::time_from_start( ) const { return trajectory_start_time_; }
351 bool Trajectory::has_trajectory_msg( ) const { return trajectory_msg_.get( ) != nullptr; }
} // namespace joint_trajectory_controller
1 // Copyright 2020 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include "controller_manager/controller_manager.hpp"
#include "hardware_interface/resource_manager.hpp"
#include "rclcpp/executor.hpp"
#include "rclcpp/executors/single_threaded_executor.hpp"
#include "rclcpp/utilities.hpp"
#include "ros2_control_test_assets/descriptions.hpp"
25 TEST( TestLoadJointStateController, load_controller )
{
rclcpp::init( 0, nullptr );
std::shared_ptr<rclcpp::Executor> executor =
std::make_shared<rclcpp::executors::SingleThreadedExecutor>( );
controller_manager::ControllerManager cm(
std::make_unique<hardware_interface::ResourceManager>(
ros2_control_test_assets::minimal_robot_urdf ),
executor, "test_controller_manager" );
ASSERT_NO_THROW( cm.load_controller(
"test_joint_trajectory_controller", "joint_trajectory_controller/JointTrajectoryController" ) );
rclcpp::shutdown( );
}
1 // Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <chrono>
#include <cmath>
#include <memory>
#include <vector>
#include "gtest/gtest.h"
#include "builtin_interfaces/msg/duration.hpp"
#include "builtin_interfaces/msg/time.hpp"
#include "joint_trajectory_controller/trajectory.hpp"
#include "rclcpp/clock.hpp"
#include "rclcpp/duration.hpp"
#include "rclcpp/time.hpp"
#include "std_msgs/msg/header.hpp"
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
using namespace joint_trajectory_controller::interpolation_methods; // NOLINT
using namespace std::chrono_literals;
namespace
{
// Floating-point value comparison threshold
const double EPS = 1e-8;
} // namespace
41 TEST( TestTrajectory, initialize_trajectory )
{
{
auto empty_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>( );
empty_msg->header.stamp.sec = 2;
empty_msg->header.stamp.nanosec = 2;
const rclcpp::Time empty_time = empty_msg->header.stamp;
auto traj = joint_trajectory_controller::Trajectory( empty_msg );
trajectory_msgs::msg::JointTrajectoryPoint expected_point;
joint_trajectory_controller::TrajectoryPointConstIter start, end;
traj.sample( rclcpp::Clock( ).now( ), DEFAULT_INTERPOLATION, expected_point, start, end );
EXPECT_EQ( traj.end( ), start );
EXPECT_EQ( traj.end( ), end );
EXPECT_EQ( empty_time, traj.time_from_start( ) );
}
{
auto empty_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>( );
empty_msg->header.stamp.sec = 0;
empty_msg->header.stamp.nanosec = 0;
const auto now = rclcpp::Clock( ).now( );
auto traj = joint_trajectory_controller::Trajectory( empty_msg );
const auto traj_starttime = traj.time_from_start( );
trajectory_msgs::msg::JointTrajectoryPoint expected_point;
joint_trajectory_controller::TrajectoryPointConstIter start, end;
traj.sample( rclcpp::Clock( ).now( ), DEFAULT_INTERPOLATION, expected_point, start, end );
EXPECT_EQ( traj.end( ), start );
EXPECT_EQ( traj.end( ), end );
const auto allowed_delta = 10000ll;
EXPECT_LT( traj.time_from_start( ).nanoseconds( ) - now.nanoseconds( ), allowed_delta );
}
}
77 TEST( TestTrajectory, sample_trajectory_positions )
{
auto full_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>( );
full_msg->header.stamp = rclcpp::Time( 0 );
trajectory_msgs::msg::JointTrajectoryPoint p1;
p1.positions.push_back( 1.0 );
p1.time_from_start = rclcpp::Duration::from_seconds( 1.0 );
full_msg->points.push_back( p1 );
trajectory_msgs::msg::JointTrajectoryPoint p2;
p2.positions.push_back( 2.0 );
p2.time_from_start = rclcpp::Duration::from_seconds( 2.0 );
full_msg->points.push_back( p2 );
trajectory_msgs::msg::JointTrajectoryPoint p3;
p3.positions.push_back( 3.0 );
p3.time_from_start = rclcpp::Duration::from_seconds( 3.0 );
full_msg->points.push_back( p3 );
trajectory_msgs::msg::JointTrajectoryPoint point_before_msg;
point_before_msg.time_from_start = rclcpp::Duration::from_seconds( 0.0 );
point_before_msg.positions.push_back( 0.0 );
// set current state before trajectory msg was sent
const rclcpp::Time time_now = rclcpp::Clock( ).now( );
auto traj = joint_trajectory_controller::Trajectory( time_now, point_before_msg, full_msg );
trajectory_msgs::msg::JointTrajectoryPoint expected_state;
joint_trajectory_controller::TrajectoryPointConstIter start, end;
double duration_first_seg = 1.0;
double velocity = ( p1.positions[0] - point_before_msg.positions[0] ) / duration_first_seg;
// sample at trajectory starting time
{
traj.sample( time_now, DEFAULT_INTERPOLATION, expected_state, start, end );
ASSERT_EQ( traj.begin( ), start );
ASSERT_EQ( traj.begin( ), end );
EXPECT_NEAR( point_before_msg.positions[0], expected_state.positions[0], EPS );
EXPECT_NEAR( velocity, expected_state.velocities[0], EPS );
EXPECT_NEAR( 0.0, expected_state.accelerations[0], EPS );
}
// sample before time_now
{
bool result = traj.sample(
time_now - rclcpp::Duration::from_seconds( 0.5 ), DEFAULT_INTERPOLATION, expected_state, start,
end );
ASSERT_EQ( result, false );
}
// sample 0.5s after msg
{
traj.sample(
time_now + rclcpp::Duration::from_seconds( 0.5 ), DEFAULT_INTERPOLATION, expected_state, start,
end );
ASSERT_EQ( traj.begin( ), start );
ASSERT_EQ( traj.begin( ), end );
double half_current_to_p1 = ( point_before_msg.positions[0] + p1.positions[0] ) * 0.5;
EXPECT_NEAR( half_current_to_p1, expected_state.positions[0], EPS );
EXPECT_NEAR( velocity, expected_state.velocities[0], EPS );
EXPECT_NEAR( 0.0, expected_state.accelerations[0], EPS );
}
// sample 1s after msg
{
traj.sample(
time_now + rclcpp::Duration::from_seconds( 1.0 ), DEFAULT_INTERPOLATION, expected_state, start,
end );
ASSERT_EQ( traj.begin( ), start );
ASSERT_EQ( ( ++traj.begin( ) ), end );
EXPECT_NEAR( p1.positions[0], expected_state.positions[0], EPS );
EXPECT_NEAR( velocity, expected_state.velocities[0], EPS );
EXPECT_NEAR( 0.0, expected_state.accelerations[0], EPS );
}
// sample 1.5s after msg
{
traj.sample(
time_now + rclcpp::Duration::from_seconds( 1.5 ), DEFAULT_INTERPOLATION, expected_state, start,
end );
ASSERT_EQ( traj.begin( ), start );
ASSERT_EQ( ( ++traj.begin( ) ), end );
double half_p1_to_p2 = ( p1.positions[0] + p2.positions[0] ) * 0.5;
EXPECT_NEAR( half_p1_to_p2, expected_state.positions[0], EPS );
}
// sample 2.5s after msg
{
traj.sample(
time_now + rclcpp::Duration::from_seconds( 2.5 ), DEFAULT_INTERPOLATION, expected_state, start,
end );
double half_p2_to_p3 = ( p2.positions[0] + p3.positions[0] ) * 0.5;
EXPECT_NEAR( half_p2_to_p3, expected_state.positions[0], EPS );
}
// sample 3s after msg
{
traj.sample(
time_now + rclcpp::Duration::from_seconds( 3.0 ), DEFAULT_INTERPOLATION, expected_state, start,
end );
EXPECT_NEAR( p3.positions[0], expected_state.positions[0], EPS );
}
// sample past given points
{
traj.sample(
time_now + rclcpp::Duration::from_seconds( 3.125 ), DEFAULT_INTERPOLATION, expected_state,
start, end );
ASSERT_EQ( ( --traj.end( ) ), start );
ASSERT_EQ( traj.end( ), end );
EXPECT_NEAR( p3.positions[0], expected_state.positions[0], EPS );
}
}
193 TEST( TestTrajectory, interpolation_pos_vel )
{
// taken from ros1_controllers QuinticSplineSegmentTest::PosVelEnpointsSampler
// Start and end state taken from x^3 - 2x
trajectory_msgs::msg::JointTrajectoryPoint start_state;
start_state.time_from_start = rclcpp::Duration::from_seconds( 1.0 );
start_state.positions.push_back( 0.0 );
start_state.velocities.push_back( -2.0 );
start_state.accelerations.clear( );
trajectory_msgs::msg::JointTrajectoryPoint end_state;
end_state.time_from_start = rclcpp::Duration::from_seconds( 3.0 );
end_state.positions.push_back( 4.0 );
end_state.velocities.push_back( 10.0 );
end_state.accelerations.push_back( 0.0 ); // Should be ignored, start state does not specify it
auto traj = joint_trajectory_controller::Trajectory( );
rclcpp::Time time_now( 0 );
trajectory_msgs::msg::JointTrajectoryPoint expected_state;
// sample at start_time
{
traj.interpolate_between_points(
time_now + start_state.time_from_start, start_state, time_now + end_state.time_from_start,
end_state, time_now + start_state.time_from_start, expected_state );
EXPECT_NEAR( start_state.positions[0], expected_state.positions[0], EPS );
EXPECT_NEAR( start_state.velocities[0], expected_state.velocities[0], EPS );
EXPECT_NEAR( 0.0, expected_state.accelerations[0], EPS );
}
// Sample at mid-segment: Zero-crossing
{
auto t = rclcpp::Duration::from_seconds( std::sqrt( 2.0 ) );
traj.interpolate_between_points(
time_now + start_state.time_from_start, start_state, time_now + end_state.time_from_start,
end_state, time_now + start_state.time_from_start + t, expected_state );
EXPECT_NEAR( 0.0, expected_state.positions[0], EPS );
EXPECT_NEAR( 4.0, expected_state.velocities[0], EPS );
EXPECT_NEAR( 6.0 * std::sqrt( 2.0 ), expected_state.accelerations[0], EPS );
}
// sample at end_time
{
traj.interpolate_between_points(
time_now + start_state.time_from_start, start_state, time_now + end_state.time_from_start,
end_state, time_now + end_state.time_from_start, expected_state );
EXPECT_NEAR( end_state.positions[0], expected_state.positions[0], EPS );
EXPECT_NEAR( end_state.velocities[0], expected_state.velocities[0], EPS );
EXPECT_NEAR( 12.0, expected_state.accelerations[0], EPS );
}
}
247 TEST( TestTrajectory, interpolation_pos_vel_accel )
{
// taken from ros1_controllers QuinticSplineSegmentTest::PosVeAcclEnpointsSampler
// Start and end state taken from x( x-1 )( x-2 )( x-3 )( x-4 ) = x^5 -10x^4 + 35x^3 -50x^2 + 24x
trajectory_msgs::msg::JointTrajectoryPoint start_state;
start_state.time_from_start = rclcpp::Duration::from_seconds( 1.0 );
start_state.positions.push_back( 0.0 );
start_state.velocities.push_back( 24.0 );
start_state.accelerations.push_back( -100.0 );
trajectory_msgs::msg::JointTrajectoryPoint end_state;
end_state.time_from_start = rclcpp::Duration::from_seconds( 3.0 );
end_state.positions.push_back( 0.0 );
end_state.velocities.push_back( 4.0 );
end_state.accelerations.push_back( 0.0 );
auto traj = joint_trajectory_controller::Trajectory( );
rclcpp::Time time_now( 0 );
trajectory_msgs::msg::JointTrajectoryPoint expected_state;
// sample at start_time
{
traj.interpolate_between_points(
time_now + start_state.time_from_start, start_state, time_now + end_state.time_from_start,
end_state, time_now + start_state.time_from_start, expected_state );
EXPECT_NEAR( start_state.positions[0], expected_state.positions[0], EPS );
EXPECT_NEAR( start_state.velocities[0], expected_state.velocities[0], EPS );
EXPECT_NEAR( start_state.accelerations[0], expected_state.accelerations[0], EPS );
}
// Sample at mid-segment: Zero-crossing
{
auto t = rclcpp::Duration::from_seconds( 1.0 );
traj.interpolate_between_points(
time_now + start_state.time_from_start, start_state, time_now + end_state.time_from_start,
end_state, time_now + start_state.time_from_start + t, expected_state );
EXPECT_NEAR( 0.0, expected_state.positions[0], EPS );
EXPECT_NEAR( -6.0, expected_state.velocities[0], EPS );
EXPECT_NEAR( 10.0, expected_state.accelerations[0], EPS );
}
// sample at end_time
{
traj.interpolate_between_points(
time_now + start_state.time_from_start, start_state, time_now + end_state.time_from_start,
end_state, time_now + end_state.time_from_start, expected_state );
EXPECT_NEAR( end_state.positions[0], expected_state.positions[0], EPS );
EXPECT_NEAR( end_state.velocities[0], expected_state.velocities[0], EPS );
EXPECT_NEAR( end_state.accelerations[0], expected_state.accelerations[0], EPS );
}
}
301 TEST( TestTrajectory, sample_trajectory_velocity_with_interpolation )
{
auto full_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>( );
full_msg->header.stamp = rclcpp::Time( 0 );
// definitions
double time_first_seg = 1.0;
double time_second_seg = 2.0;
double time_third_seg = 3.0;
double velocity_first_seg = 1.0;
double velocity_second_seg = 2.0;
double velocity_third_seg = 1.0;
trajectory_msgs::msg::JointTrajectoryPoint p1;
p1.velocities.push_back( velocity_first_seg );
p1.time_from_start = rclcpp::Duration::from_seconds( time_first_seg );
full_msg->points.push_back( p1 );
trajectory_msgs::msg::JointTrajectoryPoint p2;
p2.velocities.push_back( velocity_second_seg );
p2.time_from_start = rclcpp::Duration::from_seconds( time_second_seg );
full_msg->points.push_back( p2 );
trajectory_msgs::msg::JointTrajectoryPoint p3;
p3.velocities.push_back( velocity_third_seg );
p3.time_from_start = rclcpp::Duration::from_seconds( time_third_seg );
full_msg->points.push_back( p3 );
trajectory_msgs::msg::JointTrajectoryPoint point_before_msg;
point_before_msg.time_from_start = rclcpp::Duration::from_seconds( 0.0 );
point_before_msg.positions.push_back( 0.0 );
point_before_msg.velocities.push_back( 0.0 );
// set current state before trajectory msg was sent
const rclcpp::Time time_now = rclcpp::Clock( ).now( );
auto traj = joint_trajectory_controller::Trajectory( time_now, point_before_msg, full_msg );
trajectory_msgs::msg::JointTrajectoryPoint expected_state;
joint_trajectory_controller::TrajectoryPointConstIter start, end;
// sample at trajectory starting time
{
traj.sample( time_now, DEFAULT_INTERPOLATION, expected_state, start, end );
EXPECT_EQ( traj.begin( ), start );
EXPECT_EQ( traj.begin( ), end );
EXPECT_NEAR( point_before_msg.positions[0], expected_state.positions[0], EPS );
EXPECT_NEAR( point_before_msg.velocities[0], expected_state.velocities[0], EPS );
EXPECT_NEAR( ( velocity_first_seg / time_first_seg ), expected_state.accelerations[0], EPS );
}
// sample before time_now
{
bool result = traj.sample(
time_now - rclcpp::Duration::from_seconds( 0.5 ), DEFAULT_INTERPOLATION, expected_state, start,
end );
EXPECT_EQ( result, false );
}
// sample 0.5s after msg
{
traj.sample(
time_now + rclcpp::Duration::from_seconds( 0.5 ), DEFAULT_INTERPOLATION, expected_state, start,
end );
EXPECT_EQ( traj.begin( ), start );
EXPECT_EQ( traj.begin( ), end );
double half_current_to_p1 =
point_before_msg.positions[0] +
( point_before_msg.velocities[0] +
( ( point_before_msg.velocities[0] + p1.velocities[0] ) / 2 - point_before_msg.velocities[0] ) /
2 ) *
0.5;
EXPECT_NEAR( half_current_to_p1, expected_state.positions[0], EPS );
EXPECT_NEAR( p1.velocities[0] / 2, expected_state.velocities[0], EPS );
EXPECT_NEAR( ( velocity_first_seg / time_first_seg ), expected_state.accelerations[0], EPS );
}
// sample 1s after msg
double position_first_seg =
point_before_msg.positions[0] + ( 0.0 + p1.velocities[0] ) / 2 * time_first_seg;
{
traj.sample(
time_now + rclcpp::Duration::from_seconds( 1.0 ), DEFAULT_INTERPOLATION, expected_state, start,
end );
EXPECT_EQ( traj.begin( ), start );
EXPECT_EQ( ( ++traj.begin( ) ), end );
EXPECT_NEAR( position_first_seg, expected_state.positions[0], EPS );
EXPECT_NEAR( p1.velocities[0], expected_state.velocities[0], EPS );
EXPECT_NEAR(
( velocity_second_seg - velocity_first_seg / ( time_second_seg - time_first_seg ) ),
expected_state.accelerations[0], EPS );
}
// sample 1.5s after msg
{
traj.sample(
time_now + rclcpp::Duration::from_seconds( 1.5 ), DEFAULT_INTERPOLATION, expected_state, start,
end );
EXPECT_EQ( traj.begin( ), start );
EXPECT_EQ( ( ++traj.begin( ) ), end );
double half_p1_to_p2 =
position_first_seg +
( p1.velocities[0] + ( ( p1.velocities[0] + p2.velocities[0] ) / 2 - p1.velocities[0] ) / 2 ) * 0.5;
EXPECT_NEAR( half_p1_to_p2, expected_state.positions[0], EPS );
double half_p1_to_p2_vel = ( p1.velocities[0] + p2.velocities[0] ) / 2;
EXPECT_NEAR( half_p1_to_p2_vel, expected_state.velocities[0], EPS );
EXPECT_NEAR(
( velocity_second_seg - velocity_first_seg / ( time_second_seg - time_first_seg ) ),
expected_state.accelerations[0], EPS );
}
// sample 2s after msg
double position_second_seg = position_first_seg + ( p1.velocities[0] + p2.velocities[0] ) / 2 *
( time_second_seg - time_first_seg );
{
traj.sample(
time_now + rclcpp::Duration::from_seconds( 2 ), DEFAULT_INTERPOLATION, expected_state, start,
end );
EXPECT_EQ( ( ++traj.begin( ) ), start );
EXPECT_EQ( ( --traj.end( ) ), end );
EXPECT_NEAR( position_second_seg, expected_state.positions[0], EPS );
EXPECT_NEAR( p2.velocities[0], expected_state.velocities[0], EPS );
EXPECT_NEAR(
( velocity_third_seg - velocity_second_seg / ( time_third_seg - time_second_seg ) ),
expected_state.accelerations[0], EPS );
}
// sample 2.5s after msg
{
traj.sample(
time_now + rclcpp::Duration::from_seconds( 2.5 ), DEFAULT_INTERPOLATION, expected_state, start,
end );
EXPECT_EQ( ( ++traj.begin( ) ), start );
EXPECT_EQ( ( --traj.end( ) ), end );
double half_p2_to_p3 =
position_second_seg +
( p2.velocities[0] + ( ( p2.velocities[0] + p3.velocities[0] ) / 2 - p2.velocities[0] ) / 2 ) * 0.5;
EXPECT_NEAR( half_p2_to_p3, expected_state.positions[0], EPS );
double half_p2_to_p3_vel = ( p2.velocities[0] + p3.velocities[0] ) / 2;
EXPECT_NEAR( half_p2_to_p3_vel, expected_state.velocities[0], EPS );
EXPECT_NEAR(
( velocity_third_seg - velocity_second_seg / ( time_third_seg - time_second_seg ) ),
expected_state.accelerations[0], EPS );
}
// sample 3s after msg
double position_third_seg = position_second_seg + ( p2.velocities[0] + p3.velocities[0] ) / 2 *
( time_third_seg - time_second_seg );
{
traj.sample(
time_now + rclcpp::Duration::from_seconds( 3.0 ), DEFAULT_INTERPOLATION, expected_state, start,
end );
EXPECT_EQ( ( --traj.end( ) ), start );
EXPECT_EQ( traj.end( ), end );
EXPECT_NEAR( position_third_seg, expected_state.positions[0], EPS );
EXPECT_NEAR( p3.velocities[0], expected_state.velocities[0], EPS );
// the goal is reached so no acceleration anymore
EXPECT_NEAR( 0, expected_state.accelerations[0], EPS );
}
// sample past given points - movement virtually stops
{
traj.sample(
time_now + rclcpp::Duration::from_seconds( 3.125 ), DEFAULT_INTERPOLATION, expected_state,
start, end );
EXPECT_EQ( ( --traj.end( ) ), start );
EXPECT_EQ( traj.end( ), end );
EXPECT_NEAR( position_third_seg, expected_state.positions[0], EPS );
EXPECT_NEAR( p3.velocities[0], expected_state.velocities[0], EPS );
EXPECT_NEAR( 0.0, expected_state.accelerations[0], EPS );
}
}
// This test is added because previous one behaved strange if
// "point_before_msg.velocities.push_back( 0.0 );" was not defined
475 TEST( TestTrajectory, sample_trajectory_velocity_with_interpolation_strange_without_vel )
{
auto full_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>( );
full_msg->header.stamp = rclcpp::Time( 0 );
// definitions
double time_first_seg = 1.0;
double time_second_seg = 2.0;
double time_third_seg = 3.0;
double velocity_first_seg = 1.0;
double velocity_second_seg = 2.0;
double velocity_third_seg = 1.0;
trajectory_msgs::msg::JointTrajectoryPoint p1;
p1.velocities.push_back( velocity_first_seg );
p1.time_from_start = rclcpp::Duration::from_seconds( time_first_seg );
full_msg->points.push_back( p1 );
trajectory_msgs::msg::JointTrajectoryPoint p2;
p2.velocities.push_back( velocity_second_seg );
p2.time_from_start = rclcpp::Duration::from_seconds( time_second_seg );
full_msg->points.push_back( p2 );
trajectory_msgs::msg::JointTrajectoryPoint p3;
p3.velocities.push_back( velocity_third_seg );
p3.time_from_start = rclcpp::Duration::from_seconds( time_third_seg );
full_msg->points.push_back( p3 );
trajectory_msgs::msg::JointTrajectoryPoint point_before_msg;
point_before_msg.time_from_start = rclcpp::Duration::from_seconds( 0.0 );
point_before_msg.positions.push_back( 0.0 );
// set current state before trajectory msg was sent
const rclcpp::Time time_now = rclcpp::Clock( ).now( );
auto traj = joint_trajectory_controller::Trajectory( time_now, point_before_msg, full_msg );
trajectory_msgs::msg::JointTrajectoryPoint expected_state;
joint_trajectory_controller::TrajectoryPointConstIter start, end;
// sample at trajectory starting time
{
traj.sample( time_now, DEFAULT_INTERPOLATION, expected_state, start, end );
EXPECT_EQ( traj.begin( ), start );
EXPECT_EQ( traj.begin( ), end );
EXPECT_NEAR( point_before_msg.positions[0], expected_state.positions[0], EPS );
EXPECT_NEAR( 0.0, expected_state.velocities[0], EPS );
// is 0 because point_before_msg does not have velocity defined
EXPECT_NEAR( 1.0, expected_state.accelerations[0], EPS );
}
// sample before time_now
{
bool result = traj.sample(
time_now - rclcpp::Duration::from_seconds( 0.5 ), DEFAULT_INTERPOLATION, expected_state, start,
end );
EXPECT_EQ( result, false );
}
// sample 0.5s after msg
{
traj.sample(
time_now + rclcpp::Duration::from_seconds( 0.5 ), DEFAULT_INTERPOLATION, expected_state, start,
end );
EXPECT_EQ( traj.begin( ), start );
EXPECT_EQ( traj.begin( ), end );
// double half_current_to_p1 = point_before_msg.positions[0] +
// ( point_before_msg.velocities[0] +
// ( ( point_before_msg.velocities[0] + p1.velocities[0] ) / 2 -
// point_before_msg.velocities[0] ) / 2 ) * 0.5;
double half_current_to_p1 =
point_before_msg.positions[0] + ( 0.0 + ( ( 0.0 + p1.velocities[0] ) / 2 - 0.0 ) / 2 ) * 0.5;
EXPECT_NEAR( half_current_to_p1, expected_state.positions[0], EPS );
EXPECT_NEAR( p1.velocities[0] / 2, expected_state.velocities[0], EPS );
EXPECT_NEAR( 1.0, expected_state.accelerations[0], EPS );
}
// sample 1s after msg
double position_first_seg =
point_before_msg.positions[0] + ( 0.0 + p1.velocities[0] ) / 2 * time_first_seg;
{
traj.sample(
time_now + rclcpp::Duration::from_seconds( 1.0 ), DEFAULT_INTERPOLATION, expected_state, start,
end );
EXPECT_EQ( traj.begin( ), start );
EXPECT_EQ( ( ++traj.begin( ) ), end );
EXPECT_NEAR( position_first_seg, expected_state.positions[0], EPS );
EXPECT_NEAR( p1.velocities[0], expected_state.velocities[0], EPS );
EXPECT_NEAR( 1.0, expected_state.accelerations[0], EPS );
}
}
566 TEST( TestTrajectory, sample_trajectory_acceleration_with_interpolation )
{
auto full_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>( );
full_msg->header.stamp = rclcpp::Time( 0 );
// definitions
double time_first_seg = 1.0;
double time_second_seg = 2.0;
double time_third_seg = 3.0;
double acceleration_first_seg = 1.0;
double acceleration_second_seg = 2.0;
double acceleration_third_seg = 1.0;
trajectory_msgs::msg::JointTrajectoryPoint p1;
p1.accelerations.push_back( acceleration_first_seg );
p1.time_from_start = rclcpp::Duration::from_seconds( time_first_seg );
full_msg->points.push_back( p1 );
trajectory_msgs::msg::JointTrajectoryPoint p2;
p2.accelerations.push_back( acceleration_second_seg );
p2.time_from_start = rclcpp::Duration::from_seconds( time_second_seg );
full_msg->points.push_back( p2 );
trajectory_msgs::msg::JointTrajectoryPoint p3;
p3.accelerations.push_back( acceleration_third_seg );
p3.time_from_start = rclcpp::Duration::from_seconds( time_third_seg );
full_msg->points.push_back( p3 );
trajectory_msgs::msg::JointTrajectoryPoint point_before_msg;
point_before_msg.time_from_start = rclcpp::Duration::from_seconds( 0.0 );
point_before_msg.positions.push_back( 0.0 );
point_before_msg.velocities.push_back( 0.0 );
// set current state before trajectory msg was sent
const rclcpp::Time time_now = rclcpp::Clock( ).now( );
auto traj = joint_trajectory_controller::Trajectory( time_now, point_before_msg, full_msg );
trajectory_msgs::msg::JointTrajectoryPoint expected_state;
joint_trajectory_controller::TrajectoryPointConstIter start, end;
// sample at trajectory starting time
{
traj.sample( time_now, DEFAULT_INTERPOLATION, expected_state, start, end );
EXPECT_EQ( traj.begin( ), start );
EXPECT_EQ( traj.begin( ), end );
EXPECT_NEAR( point_before_msg.positions[0], expected_state.positions[0], EPS );
EXPECT_NEAR( 0.0, expected_state.velocities[0], EPS );
// is 0 because point_before_msg does not have velocity defined
EXPECT_NEAR( 0.0, expected_state.accelerations[0], EPS );
}
// sample before time_now
{
bool result = traj.sample(
time_now - rclcpp::Duration::from_seconds( 0.5 ), DEFAULT_INTERPOLATION, expected_state, start,
end );
EXPECT_EQ( result, false );
}
// Sample only on points testing of intermediate values is too complex and not necessary
// sample 1s after msg
double velocity_first_seg =
point_before_msg.velocities[0] + ( 0.0 + p1.accelerations[0] ) / 2 * time_first_seg;
double position_first_seg =
point_before_msg.positions[0] + ( 0.0 + velocity_first_seg ) / 2 * time_first_seg;
{
traj.sample(
time_now + rclcpp::Duration::from_seconds( 1.0 ), DEFAULT_INTERPOLATION, expected_state, start,
end );
EXPECT_EQ( traj.begin( ), start );
EXPECT_EQ( ( ++traj.begin( ) ), end );
EXPECT_NEAR( position_first_seg, expected_state.positions[0], EPS );
EXPECT_NEAR( velocity_first_seg, expected_state.velocities[0], EPS );
EXPECT_NEAR( p1.accelerations[0], expected_state.accelerations[0], EPS );
}
// sample 2s after msg
double velocity_second_seg = velocity_first_seg + ( p1.accelerations[0] + p2.accelerations[0] ) /
2 * ( time_second_seg - time_first_seg );
double position_second_seg = position_first_seg + ( velocity_first_seg + velocity_second_seg ) / 2 *
( time_second_seg - time_first_seg );
{
traj.sample(
time_now + rclcpp::Duration::from_seconds( 2 ), DEFAULT_INTERPOLATION, expected_state, start,
end );
EXPECT_EQ( ( ++traj.begin( ) ), start );
EXPECT_EQ( ( --traj.end( ) ), end );
EXPECT_NEAR( position_second_seg, expected_state.positions[0], EPS );
EXPECT_NEAR( velocity_second_seg, expected_state.velocities[0], EPS );
EXPECT_NEAR( p2.accelerations[0], expected_state.accelerations[0], EPS );
}
// sample 3s after msg
double velocity_third_seg = velocity_second_seg + ( p2.accelerations[0] + p3.accelerations[0] ) /
2 * ( time_third_seg - time_second_seg );
double position_third_seg = position_second_seg + ( velocity_second_seg + velocity_third_seg ) / 2 *
( time_third_seg - time_second_seg );
{
traj.sample(
time_now + rclcpp::Duration::from_seconds( 3.0 ), DEFAULT_INTERPOLATION, expected_state, start,
end );
EXPECT_EQ( ( --traj.end( ) ), start );
EXPECT_EQ( traj.end( ), end );
EXPECT_NEAR( position_third_seg, expected_state.positions[0], EPS );
EXPECT_NEAR( velocity_third_seg, expected_state.velocities[0], EPS );
EXPECT_NEAR( p3.accelerations[0], expected_state.accelerations[0], EPS );
}
// sample past given points - movement virtually stops
{
traj.sample(
time_now + rclcpp::Duration::from_seconds( 3.125 ), DEFAULT_INTERPOLATION, expected_state,
start, end );
EXPECT_EQ( ( --traj.end( ) ), start );
EXPECT_EQ( traj.end( ), end );
EXPECT_NEAR( position_third_seg, expected_state.positions[0], EPS );
EXPECT_NEAR( velocity_third_seg, expected_state.velocities[0], EPS );
EXPECT_NEAR( p3.accelerations[0], expected_state.accelerations[0], EPS );
}
}
688 TEST( TestTrajectory, skip_interpolation )
{
// Simple passthrough without extra interpolation
{
const InterpolationMethod no_interpolation = InterpolationMethod::NONE;
auto full_msg = std::make_shared<trajectory_msgs::msg::JointTrajectory>( );
full_msg->header.stamp = rclcpp::Time( 0 );
trajectory_msgs::msg::JointTrajectoryPoint p1;
p1.positions.push_back( 1.0 );
p1.time_from_start = rclcpp::Duration::from_seconds( 1.0 );
full_msg->points.push_back( p1 );
trajectory_msgs::msg::JointTrajectoryPoint p2;
p2.positions.push_back( 2.0 );
p2.time_from_start = rclcpp::Duration::from_seconds( 2.0 );
full_msg->points.push_back( p2 );
trajectory_msgs::msg::JointTrajectoryPoint p3;
p3.positions.push_back( 3.0 );
p3.time_from_start = rclcpp::Duration::from_seconds( 3.0 );
full_msg->points.push_back( p3 );
trajectory_msgs::msg::JointTrajectoryPoint point_before_msg;
point_before_msg.time_from_start = rclcpp::Duration::from_seconds( 0.0 );
point_before_msg.positions.push_back( 0.0 );
// set current state before trajectory msg was sent
const rclcpp::Time time_now = rclcpp::Clock( ).now( );
auto traj = joint_trajectory_controller::Trajectory( time_now, point_before_msg, full_msg );
trajectory_msgs::msg::JointTrajectoryPoint expected_state;
joint_trajectory_controller::TrajectoryPointConstIter start, end;
// sample at trajectory starting time
{
traj.sample( time_now, no_interpolation, expected_state, start, end );
ASSERT_EQ( traj.begin( ), start );
ASSERT_EQ( traj.begin( ), end );
EXPECT_NEAR( point_before_msg.positions[0], expected_state.positions[0], EPS );
// There were no vels/accels in the input, so they should remain empty
EXPECT_EQ(
static_cast<std::make_unsigned<decltype( 0 )>::type>( 0 ), expected_state.velocities.size( ) );
EXPECT_EQ(
static_cast<std::make_unsigned<decltype( 0 )>::type>( 0 ), expected_state.accelerations.size( ) );
}
// sample before time_now
{
bool result = traj.sample(
time_now - rclcpp::Duration::from_seconds( 0.5 ), no_interpolation, expected_state, start,
end );
ASSERT_EQ( result, false );
}
// sample 0.5s after msg
{
traj.sample(
time_now + rclcpp::Duration::from_seconds( 0.5 ), no_interpolation, expected_state, start,
end );
ASSERT_EQ( traj.begin( ), start );
ASSERT_EQ( traj.begin( ), end );
// For passthrough, this should just return the first waypoint
EXPECT_NEAR( point_before_msg.positions[0], expected_state.positions[0], EPS );
// There were no vels/accels in the input, so they should remain empty
EXPECT_EQ(
static_cast<std::make_unsigned<decltype( 0 )>::type>( 0 ), expected_state.velocities.size( ) );
EXPECT_EQ(
static_cast<std::make_unsigned<decltype( 0 )>::type>( 0 ), expected_state.accelerations.size( ) );
}
// sample 1s after msg
{
traj.sample(
time_now + rclcpp::Duration::from_seconds( 1.0 ), no_interpolation, expected_state, start,
end );
ASSERT_EQ( traj.begin( ), start );
ASSERT_EQ( ( ++traj.begin( ) ), end );
EXPECT_NEAR( p2.positions[0], expected_state.positions[0], EPS );
// There were no vels/accels in the input, so they should remain empty
EXPECT_EQ(
static_cast<std::make_unsigned<decltype( 0 )>::type>( 0 ), expected_state.velocities.size( ) );
EXPECT_EQ(
static_cast<std::make_unsigned<decltype( 0 )>::type>( 0 ), expected_state.accelerations.size( ) );
}
// sample 1.5s after msg
{
traj.sample(
time_now + rclcpp::Duration::from_seconds( 1.5 ), no_interpolation, expected_state, start,
end );
ASSERT_EQ( traj.begin( ), start );
ASSERT_EQ( ( ++traj.begin( ) ), end );
EXPECT_NEAR( p2.positions[0], expected_state.positions[0], EPS );
}
// sample 2.5s after msg
{
traj.sample(
time_now + rclcpp::Duration::from_seconds( 2.5 ), no_interpolation, expected_state, start,
end );
EXPECT_NEAR( p3.positions[0], expected_state.positions[0], EPS );
}
// sample 3s after msg
{
traj.sample(
time_now + rclcpp::Duration::from_seconds( 3.0 ), no_interpolation, expected_state, start,
end );
EXPECT_NEAR( p3.positions[0], expected_state.positions[0], EPS );
}
// sample past given points
{
traj.sample(
time_now + rclcpp::Duration::from_seconds( 3.125 ), no_interpolation, expected_state, start,
end );
ASSERT_EQ( ( --traj.end( ) ), start );
ASSERT_EQ( traj.end( ), end );
EXPECT_NEAR( p3.positions[0], expected_state.positions[0], EPS );
}
}
}
// Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef _MSC_VER
#include <cxxabi.h>
#endif
#include <algorithm>
#include <chrono>
#include <functional>
#include <future>
#include <memory>
#include <ratio>
#include <stdexcept>
#include <string>
#include <system_error>
#include <thread>
#include <vector>
#include "action_msgs/msg/goal_status_array.hpp"
#include "control_msgs/action/detail/follow_joint_trajectory__struct.hpp"
#include "controller_interface/controller_interface.hpp"
#include "gtest/gtest.h"
#include "hardware_interface/resource_manager.hpp"
#include "joint_trajectory_controller/joint_trajectory_controller.hpp"
#include "rclcpp/clock.hpp"
#include "rclcpp/duration.hpp"
#include "rclcpp/executors/multi_threaded_executor.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/parameter.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp/utilities.hpp"
#include "rclcpp_action/client.hpp"
#include "rclcpp_action/client_goal_handle.hpp"
#include "rclcpp_action/create_client.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "test_trajectory_controller_utils.hpp"
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
using std::placeholders::_1;
using std::placeholders::_2;
using test_trajectory_controllers::TestableJointTrajectoryController;
using test_trajectory_controllers::TrajectoryControllerTest;
using trajectory_msgs::msg::JointTrajectoryPoint;
58 class TestTrajectoryActions : public TrajectoryControllerTest
{
protected:
61 void SetUp( )
{
TrajectoryControllerTest::SetUp( );
goal_options_.result_callback =
std::bind( &TestTrajectoryActions::common_result_response, this, _1 );
goal_options_.feedback_callback = nullptr;
}
69 void SetUpExecutor( const std::vector<rclcpp::Parameter> & parameters = {} )
{
setup_executor_ = true;
executor_ = std::make_unique<rclcpp::executors::MultiThreadedExecutor>( );
SetUpAndActivateTrajectoryController( true, parameters );
executor_->add_node( traj_controller_->get_node( )->get_node_base_interface( ) );
SetUpActionClient( );
executor_->add_node( node_->get_node_base_interface( ) );
executor_future_handle_ = std::async( std::launch::async, [&]( ) -> void { executor_->spin( ); } );
}
86 void SetUpControllerHardware( )
{
setup_controller_hw_ = true;
controller_hw_thread_ = std::thread( [&]( ) {
// controller hardware cycle update loop
auto start_time = rclcpp::Clock( ).now( );
rclcpp::Duration wait = rclcpp::Duration::from_seconds( 2.0 );
auto end_time = start_time + wait;
while ( rclcpp::Clock( ).now( ) < end_time )
{
traj_controller_->update( rclcpp::Clock( ).now( ), rclcpp::Clock( ).now( ) - start_time );
}
} );
// sometimes doesn't receive calls when we don't sleep
std::this_thread::sleep_for( std::chrono::milliseconds( 300 ) );
}
105 void SetUpActionClient( )
{
action_client_ = rclcpp_action::create_client<control_msgs::action::FollowJointTrajectory>(
node_->get_node_base_interface( ), node_->get_node_graph_interface( ),
node_->get_node_logging_interface( ), node_->get_node_waitables_interface( ),
controller_name_ + "/follow_joint_trajectory" );
bool response = action_client_->wait_for_action_server( std::chrono::seconds( 1 ) );
if ( !response )
{
throw std::runtime_error( "could not get action server" );
}
}
119 static void TearDownTestCase( ) { rclcpp::shutdown( ); }
121 void TearDown( )
{
TearDownControllerHardware( );
TearDownExecutor( );
}
127 void TearDownExecutor( )
{
if ( setup_executor_ )
{
setup_executor_ = false;
executor_->cancel( );
executor_future_handle_.wait( );
}
}
137 void TearDownControllerHardware( )
{
if ( setup_controller_hw_ )
{
setup_controller_hw_ = false;
if ( controller_hw_thread_.joinable( ) )
{
controller_hw_thread_.join( );
}
}
}
using FollowJointTrajectoryMsg = control_msgs::action::FollowJointTrajectory;
using GoalHandle = rclcpp_action::ClientGoalHandle<FollowJointTrajectoryMsg>;
using GoalOptions = rclcpp_action::Client<FollowJointTrajectoryMsg>::SendGoalOptions;
std::shared_future<typename GoalHandle::SharedPtr> sendActionGoal(
const std::vector<JointTrajectoryPoint> & points, double timeout, const GoalOptions & opt )
{
control_msgs::action::FollowJointTrajectory_Goal goal_msg;
goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds( timeout );
goal_msg.trajectory.joint_names = joint_names_;
goal_msg.trajectory.points = points;
return action_client_->async_send_goal( goal_msg, opt );
}
rclcpp_action::Client<FollowJointTrajectoryMsg>::SharedPtr action_client_;
rclcpp_action::ResultCode common_resultcode_ = rclcpp_action::ResultCode::UNKNOWN;
int common_action_result_code_ = control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL;
bool setup_executor_ = false;
rclcpp::executors::MultiThreadedExecutor::UniquePtr executor_;
std::future<void> executor_future_handle_;
bool setup_controller_hw_ = false;
std::thread controller_hw_thread_;
GoalOptions goal_options_;
public:
void common_result_response( const GoalHandle::WrappedResult & result )
{
RCLCPP_DEBUG(
node_->get_logger( ), "common_result_response time: %f", rclcpp::Clock( ).now( ).seconds( ) );
common_resultcode_ = result.code;
common_action_result_code_ = result.result->error_code;
switch ( result.code )
{
case rclcpp_action::ResultCode::SUCCEEDED:
break;
case rclcpp_action::ResultCode::ABORTED:
RCLCPP_DEBUG( node_->get_logger( ), "Goal was aborted" );
return;
case rclcpp_action::ResultCode::CANCELED:
RCLCPP_DEBUG( node_->get_logger( ), "Goal was canceled" );
return;
default:
RCLCPP_DEBUG( node_->get_logger( ), "Unknown result code" );
return;
}
}
};
TEST_F( TestTrajectoryActions, test_success_single_point_sendgoal )
{
SetUpExecutor( );
SetUpControllerHardware( );
std::shared_future<typename GoalHandle::SharedPtr> gh_future;
// send goal
{
std::vector<JointTrajectoryPoint> points;
JointTrajectoryPoint point;
point.time_from_start = rclcpp::Duration::from_seconds( 0.5 );
point.positions.resize( joint_names_.size( ) );
point.positions[0] = 1.0;
point.positions[1] = 2.0;
point.positions[2] = 3.0;
points.push_back( point );
gh_future = sendActionGoal( points, 1.0, goal_options_ );
}
controller_hw_thread_.join( );
EXPECT_TRUE( gh_future.get( ) );
EXPECT_EQ( rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_ );
EXPECT_EQ( 1.0, joint_pos_[0] );
EXPECT_EQ( 2.0, joint_pos_[1] );
EXPECT_EQ( 3.0, joint_pos_[2] );
}
TEST_F( TestTrajectoryActions, test_success_multi_point_sendgoal )
{
SetUpExecutor( );
SetUpControllerHardware( );
// add feedback
bool feedback_recv = false;
goal_options_.feedback_callback =
[&](
rclcpp_action::ClientGoalHandle<FollowJointTrajectoryMsg>::SharedPtr,
const std::shared_ptr<const FollowJointTrajectoryMsg::Feedback> ) { feedback_recv = true; };
std::shared_future<typename GoalHandle::SharedPtr> gh_future;
// send goal with multiple points
{
std::vector<JointTrajectoryPoint> points;
JointTrajectoryPoint point1;
point1.time_from_start = rclcpp::Duration::from_seconds( 0.2 );
point1.positions.resize( joint_names_.size( ) );
point1.positions[0] = 4.0;
point1.positions[1] = 5.0;
point1.positions[2] = 6.0;
points.push_back( point1 );
JointTrajectoryPoint point2;
point2.time_from_start = rclcpp::Duration::from_seconds( 0.3 );
point2.positions.resize( joint_names_.size( ) );
point2.positions[0] = 7.0;
point2.positions[1] = 8.0;
point2.positions[2] = 9.0;
points.push_back( point2 );
gh_future = sendActionGoal( points, 1.0, goal_options_ );
}
controller_hw_thread_.join( );
EXPECT_TRUE( feedback_recv );
EXPECT_TRUE( gh_future.get( ) );
EXPECT_EQ( rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_ );
EXPECT_NEAR( 7.0, joint_pos_[0], COMMON_THRESHOLD );
EXPECT_NEAR( 8.0, joint_pos_[1], COMMON_THRESHOLD );
EXPECT_NEAR( 9.0, joint_pos_[2], COMMON_THRESHOLD );
}
TEST_F( TestTrajectoryActions, test_goal_tolerances_single_point_success )
{
// set tolerance parameters
std::vector<rclcpp::Parameter> params = {
rclcpp::Parameter( "constraints.joint1.goal", 0.1 ),
rclcpp::Parameter( "constraints.joint2.goal", 0.1 ),
rclcpp::Parameter( "constraints.joint3.goal", 0.1 )};
SetUpExecutor( params );
SetUpControllerHardware( );
std::shared_future<typename GoalHandle::SharedPtr> gh_future;
// send goal
{
std::vector<JointTrajectoryPoint> points;
JointTrajectoryPoint point;
point.time_from_start = rclcpp::Duration::from_seconds( 0.5 );
point.positions.resize( joint_names_.size( ) );
point.positions[0] = 1.0;
point.positions[1] = 2.0;
point.positions[2] = 3.0;
points.push_back( point );
gh_future = sendActionGoal( points, 1.0, goal_options_ );
}
controller_hw_thread_.join( );
EXPECT_TRUE( gh_future.get( ) );
EXPECT_EQ( rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_ );
EXPECT_EQ(
control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_ );
EXPECT_NEAR( 1.0, joint_pos_[0], COMMON_THRESHOLD );
EXPECT_NEAR( 2.0, joint_pos_[1], COMMON_THRESHOLD );
EXPECT_NEAR( 3.0, joint_pos_[2], COMMON_THRESHOLD );
}
TEST_F( TestTrajectoryActions, test_goal_tolerances_multi_point_success )
{
// set tolerance parameters
std::vector<rclcpp::Parameter> params = {
rclcpp::Parameter( "constraints.joint1.goal", 0.1 ),
rclcpp::Parameter( "constraints.joint2.goal", 0.1 ),
rclcpp::Parameter( "constraints.joint3.goal", 0.1 )};
SetUpExecutor( params );
SetUpControllerHardware( );
// add feedback
bool feedback_recv = false;
goal_options_.feedback_callback =
[&](
rclcpp_action::ClientGoalHandle<FollowJointTrajectoryMsg>::SharedPtr,
const std::shared_ptr<const FollowJointTrajectoryMsg::Feedback> ) { feedback_recv = true; };
std::shared_future<typename GoalHandle::SharedPtr> gh_future;
// send goal with multiple points
{
std::vector<JointTrajectoryPoint> points;
JointTrajectoryPoint point1;
point1.time_from_start = rclcpp::Duration::from_seconds( 0.2 );
point1.positions.resize( joint_names_.size( ) );
point1.positions[0] = 4.0;
point1.positions[1] = 5.0;
point1.positions[2] = 6.0;
points.push_back( point1 );
JointTrajectoryPoint point2;
point2.time_from_start = rclcpp::Duration::from_seconds( 0.3 );
point2.positions.resize( joint_names_.size( ) );
point2.positions[0] = 7.0;
point2.positions[1] = 8.0;
point2.positions[2] = 9.0;
points.push_back( point2 );
gh_future = sendActionGoal( points, 1.0, goal_options_ );
}
controller_hw_thread_.join( );
EXPECT_TRUE( feedback_recv );
EXPECT_TRUE( gh_future.get( ) );
EXPECT_EQ( rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_ );
EXPECT_EQ(
control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_ );
EXPECT_NEAR( 7.0, joint_pos_[0], COMMON_THRESHOLD );
EXPECT_NEAR( 8.0, joint_pos_[1], COMMON_THRESHOLD );
EXPECT_NEAR( 9.0, joint_pos_[2], COMMON_THRESHOLD );
}
TEST_F( TestTrajectoryActions, test_state_tolerances_fail )
{
// set joint tolerance parameters
const double state_tol = 0.0001;
std::vector<rclcpp::Parameter> params = {
rclcpp::Parameter( "constraints.joint1.trajectory", state_tol ),
rclcpp::Parameter( "constraints.joint2.trajectory", state_tol ),
rclcpp::Parameter( "constraints.joint3.trajectory", state_tol )};
SetUpExecutor( params );
SetUpControllerHardware( );
std::shared_future<typename GoalHandle::SharedPtr> gh_future;
// send goal
{
std::vector<JointTrajectoryPoint> points;
JointTrajectoryPoint point1;
point1.time_from_start = rclcpp::Duration::from_seconds( 0.0 );
point1.positions.resize( joint_names_.size( ) );
point1.positions[0] = 4.0;
point1.positions[1] = 5.0;
point1.positions[2] = 6.0;
points.push_back( point1 );
JointTrajectoryPoint point2;
point2.time_from_start = rclcpp::Duration::from_seconds( 0.1 );
point2.positions.resize( joint_names_.size( ) );
point2.positions[0] = 7.0;
point2.positions[1] = 8.0;
point2.positions[2] = 9.0;
points.push_back( point2 );
gh_future = sendActionGoal( points, 1.0, goal_options_ );
}
controller_hw_thread_.join( );
EXPECT_TRUE( gh_future.get( ) );
EXPECT_EQ( rclcpp_action::ResultCode::ABORTED, common_resultcode_ );
EXPECT_EQ(
control_msgs::action::FollowJointTrajectory_Result::PATH_TOLERANCE_VIOLATED,
common_action_result_code_ );
// run an update, it should be holding
traj_controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
EXPECT_NEAR( INITIAL_POS_JOINT1, joint_pos_[0], COMMON_THRESHOLD );
EXPECT_NEAR( INITIAL_POS_JOINT2, joint_pos_[1], COMMON_THRESHOLD );
EXPECT_NEAR( INITIAL_POS_JOINT3, joint_pos_[2], COMMON_THRESHOLD );
}
TEST_F( TestTrajectoryActions, test_goal_tolerances_fail )
{
// set joint tolerance parameters
const double goal_tol = 0.1;
// set very small goal_time so that goal_time is violated
const double goal_time = 0.000001;
std::vector<rclcpp::Parameter> params = {
rclcpp::Parameter( "constraints.joint1.goal", goal_tol ),
rclcpp::Parameter( "constraints.joint2.goal", goal_tol ),
rclcpp::Parameter( "constraints.joint3.goal", goal_tol ),
rclcpp::Parameter( "constraints.goal_time", goal_time )};
SetUpExecutor( params );
SetUpControllerHardware( );
const double init_pos1 = joint_pos_[0];
const double init_pos2 = joint_pos_[1];
const double init_pos3 = joint_pos_[2];
std::shared_future<typename GoalHandle::SharedPtr> gh_future;
// send goal
{
std::vector<JointTrajectoryPoint> points;
JointTrajectoryPoint point;
point.time_from_start = rclcpp::Duration::from_seconds( 0.0 );
point.positions.resize( joint_names_.size( ) );
point.positions[0] = 4.0;
point.positions[1] = 5.0;
point.positions[2] = 6.0;
points.push_back( point );
gh_future = sendActionGoal( points, 1.0, goal_options_ );
}
controller_hw_thread_.join( );
EXPECT_TRUE( gh_future.get( ) );
EXPECT_EQ( rclcpp_action::ResultCode::ABORTED, common_resultcode_ );
EXPECT_EQ(
control_msgs::action::FollowJointTrajectory_Result::GOAL_TOLERANCE_VIOLATED,
common_action_result_code_ );
// run an update, it should be holding
traj_controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
EXPECT_NEAR( init_pos1, joint_pos_[0], COMMON_THRESHOLD );
EXPECT_NEAR( init_pos2, joint_pos_[1], COMMON_THRESHOLD );
EXPECT_NEAR( init_pos3, joint_pos_[2], COMMON_THRESHOLD );
}
TEST_F( TestTrajectoryActions, test_no_time_from_start_state_tolerance_fail )
{
// set joint tolerance parameters
const double state_tol = 0.0001;
std::vector<rclcpp::Parameter> params = {
rclcpp::Parameter( "constraints.joint1.trajectory", state_tol ),
rclcpp::Parameter( "constraints.joint2.trajectory", state_tol ),
rclcpp::Parameter( "constraints.joint3.trajectory", state_tol )};
SetUpExecutor( params );
SetUpControllerHardware( );
const double init_pos1 = joint_pos_[0];
const double init_pos2 = joint_pos_[1];
const double init_pos3 = joint_pos_[2];
std::shared_future<typename GoalHandle::SharedPtr> gh_future;
// send goal
{
std::vector<JointTrajectoryPoint> points;
JointTrajectoryPoint point;
point.time_from_start = rclcpp::Duration::from_seconds( 0.0 );
point.positions.resize( joint_names_.size( ) );
point.positions[0] = 4.0;
point.positions[1] = 5.0;
point.positions[2] = 6.0;
points.push_back( point );
gh_future = sendActionGoal( points, 1.0, goal_options_ );
}
controller_hw_thread_.join( );
EXPECT_TRUE( gh_future.get( ) );
EXPECT_EQ( rclcpp_action::ResultCode::ABORTED, common_resultcode_ );
EXPECT_EQ(
control_msgs::action::FollowJointTrajectory_Result::PATH_TOLERANCE_VIOLATED,
common_action_result_code_ );
// run an update, it should be holding
traj_controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
EXPECT_NEAR( init_pos1, joint_pos_[0], COMMON_THRESHOLD );
EXPECT_NEAR( init_pos2, joint_pos_[1], COMMON_THRESHOLD );
EXPECT_NEAR( init_pos3, joint_pos_[2], COMMON_THRESHOLD );
}
TEST_F( TestTrajectoryActions, test_cancel_hold_position )
{
SetUpExecutor( );
SetUpControllerHardware( );
std::shared_future<typename GoalHandle::SharedPtr> gh_future;
// send goal
{
std::vector<JointTrajectoryPoint> points;
JointTrajectoryPoint point;
point.time_from_start = rclcpp::Duration::from_seconds( 1.0 );
point.positions.resize( joint_names_.size( ) );
point.positions[0] = 4.0;
point.positions[1] = 5.0;
point.positions[2] = 6.0;
points.push_back( point );
control_msgs::action::FollowJointTrajectory_Goal goal_msg;
goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds( 2.0 );
goal_msg.trajectory.joint_names = joint_names_;
goal_msg.trajectory.points = points;
// send and wait for half a second before cancel
gh_future = action_client_->async_send_goal( goal_msg, goal_options_ );
std::this_thread::sleep_for( std::chrono::milliseconds( 500 ) );
const auto goal_handle = gh_future.get( );
action_client_->async_cancel_goal( goal_handle );
}
controller_hw_thread_.join( );
EXPECT_TRUE( gh_future.get( ) );
EXPECT_EQ( rclcpp_action::ResultCode::CANCELED, common_resultcode_ );
EXPECT_EQ(
control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_ );
const double prev_pos1 = joint_pos_[0];
const double prev_pos2 = joint_pos_[1];
const double prev_pos3 = joint_pos_[2];
// run an update, it should be holding
traj_controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
EXPECT_EQ( prev_pos1, joint_pos_[0] );
EXPECT_EQ( prev_pos2, joint_pos_[1] );
EXPECT_EQ( prev_pos3, joint_pos_[2] );
}
1 // Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <stddef.h>
#include <array>
#include <chrono>
#include <future>
#include <limits>
#include <memory>
#include <stdexcept>
#include <string>
#include <system_error>
#include <thread>
#include <vector>
#include "gtest/gtest.h"
#include "builtin_interfaces/msg/duration.hpp"
#include "builtin_interfaces/msg/time.hpp"
#include "control_msgs/msg/detail/joint_trajectory_controller_state__struct.hpp"
#include "controller_interface/controller_interface.hpp"
#include "hardware_interface/resource_manager.hpp"
#include "joint_trajectory_controller/joint_trajectory_controller.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/clock.hpp"
#include "rclcpp/duration.hpp"
#include "rclcpp/executors/multi_threaded_executor.hpp"
#include "rclcpp/executors/single_threaded_executor.hpp"
#include "rclcpp/node.hpp"
#include "rclcpp/parameter.hpp"
#include "rclcpp/publisher.hpp"
#include "rclcpp/qos.hpp"
#include "rclcpp/qos_event.hpp"
#include "rclcpp/subscription.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp/utilities.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "std_msgs/msg/header.hpp"
#include "test_trajectory_controller_utils.hpp"
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
using lifecycle_msgs::msg::State;
using test_trajectory_controllers::TestableJointTrajectoryController;
using test_trajectory_controllers::TrajectoryControllerTest;
using test_trajectory_controllers::TrajectoryControllerTestParameterized;
61 bool is_same_sign( double val1, double val2 ) { return val1 * val2 >= 0.0; }
63 void spin( rclcpp::executors::MultiThreadedExecutor * exe ) { exe->spin( ); }
65 TEST_P( TrajectoryControllerTestParameterized, configure )
{
SetUpTrajectoryController( );
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node( traj_controller_->get_node( )->get_node_base_interface( ) );
const auto future_handle_ = std::async( std::launch::async, spin, &executor );
const auto state = traj_controller_->get_node( )->configure( );
ASSERT_EQ( state.id( ), State::PRIMARY_STATE_INACTIVE );
// send msg
builtin_interfaces::msg::Duration time_from_start;
time_from_start.sec = 1;
time_from_start.nanosec = 0;
std::vector<std::vector<double>> points{{{3.3, 4.4, 5.5}}};
publish( time_from_start, points );
std::this_thread::sleep_for( std::chrono::milliseconds( 10 ) );
traj_controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
// hw position == 0 because controller is not activated
EXPECT_EQ( 0.0, joint_pos_[0] );
EXPECT_EQ( 0.0, joint_pos_[1] );
EXPECT_EQ( 0.0, joint_pos_[2] );
executor.cancel( );
}
94 TEST_P( TrajectoryControllerTestParameterized, activate )
{
SetUpTrajectoryController( );
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node( traj_controller_->get_node( )->get_node_base_interface( ) );
traj_controller_->get_node( )->configure( );
ASSERT_EQ( traj_controller_->get_state( ).id( ), State::PRIMARY_STATE_INACTIVE );
auto cmd_interface_config = traj_controller_->command_interface_configuration( );
ASSERT_EQ(
cmd_interface_config.names.size( ), joint_names_.size( ) * command_interface_types_.size( ) );
auto state_interface_config = traj_controller_->state_interface_configuration( );
ASSERT_EQ(
state_interface_config.names.size( ), joint_names_.size( ) * state_interface_types_.size( ) );
ActivateTrajectoryController( );
ASSERT_EQ( traj_controller_->get_state( ).id( ), State::PRIMARY_STATE_ACTIVE );
executor.cancel( );
}
// TEST_F( TestTrajectoryController, activation ) {
// auto traj_controller = std::make_shared<ros_controllers::JointTrajectoryController>(
// joint_names_, op_mode_ );
// auto ret = traj_controller->init( test_robot_, controller_name_ );
// if ( ret != controller_interface::return_type::OK ) {
// FAIL( );
// }
//
// auto traj_node = traj_controller->get_node( );
// rclcpp::executors::MultiThreadedExecutor executor;
// executor.add_node( traj_node->get_node_base_interface( ) );
//
// auto state = traj_controller_->configure( );
// ASSERT_EQ( state.id( ), State::PRIMARY_STATE_INACTIVE );
//
// state = traj_node->activate( );
// ASSERT_EQ( state.id( ), State::PRIMARY_STATE_ACTIVE );
//
// // wait for the subscriber and publisher to completely setup
// std::this_thread::sleep_for( std::chrono::seconds( 2 ) );
//
// // send msg
// builtin_interfaces::msg::Duration time_from_start;
// time_from_start.sec = 1;
// time_from_start.nanosec = 0;
// std::vector<std::vector<double>> points {{{3.3, 4.4, 5.5}}};
// publish( time_from_start, points );
// // wait for msg is be published to the system
// std::this_thread::sleep_for( std::chrono::milliseconds( 1000 ) );
// executor.spin_once( );
//
// traj_controller->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
// resource_manager_->write( );
//
// // change in hw position
// EXPECT_EQ( 3.3, joint_pos_[0] );
// EXPECT_EQ( 4.4, joint_pos_[1] );
// EXPECT_EQ( 5.5, joint_pos_[2] );
//
// executor.cancel( );
// }
// TEST_F( TestTrajectoryController, reactivation ) {
// auto traj_controller = std::make_shared<ros_controllers::JointTrajectoryController>(
// joint_names_, op_mode_ );
// auto ret = traj_controller->init( test_robot_, controller_name_ );
// if ( ret != controller_interface::return_type::OK ) {
// FAIL( );
// }
//
// auto traj_node = traj_controller->get_node( );
// rclcpp::executors::MultiThreadedExecutor executor;
// executor.add_node( traj_node->get_node_base_interface( ) );
//
// auto state = traj_controller_->configure( );
// ASSERT_EQ( state.id( ), State::PRIMARY_STATE_INACTIVE );
//
// state = traj_node->activate( );
// ASSERT_EQ( state.id( ), State::PRIMARY_STATE_ACTIVE );
//
// // wait for the subscriber and publisher to completely setup
// std::this_thread::sleep_for( std::chrono::seconds( 2 ) );
//
// // send msg
// builtin_interfaces::msg::Duration time_from_start;
// time_from_start.sec = 1;
// time_from_start.nanosec = 0;
// // *INDENT-OFF*
// std::vector<std::vector<double>> points {
// {{3.3, 4.4, 5.5}},
// {{7.7, 8.8, 9.9}},
// {{10.10, 11.11, 12.12}}
// };
// // *INDENT-ON*
// publish( time_from_start, points );
// // wait for msg is be published to the system
// std::this_thread::sleep_for( std::chrono::milliseconds( 500 ) );
// executor.spin_once( );
//
// traj_controller->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
// resource_manager_->write( );
//
// // deactivated
// // wait so controller process the second point when deactivated
// std::this_thread::sleep_for( std::chrono::milliseconds( 500 ) );
// state = traj_controller_->deactivate( );
// ASSERT_EQ( state.id( ), State::PRIMARY_STATE_INACTIVE );
// resource_manager_->read( );
// traj_controller->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
// resource_manager_->write( );
//
// // no change in hw position
// EXPECT_EQ( 3.3, joint_pos_[0] );
// EXPECT_EQ( 4.4, joint_pos_[1] );
// EXPECT_EQ( 5.5, joint_pos_[2] );
//
// // reactivated
// // wait so controller process the third point when reactivated
// std::this_thread::sleep_for( std::chrono::milliseconds( 3000 ) );
// state = traj_node->activate( );
// ASSERT_EQ( state.id( ), State::PRIMARY_STATE_ACTIVE );
// resource_manager_->read( );
// traj_controller->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
// resource_manager_->write( );
//
// // change in hw position to 3rd point
// EXPECT_EQ( 10.10, joint_pos_[0] );
// EXPECT_EQ( 11.11, joint_pos_[1] );
// EXPECT_EQ( 12.12, joint_pos_[2] );
//
// executor.cancel( );
// }
231 TEST_P( TrajectoryControllerTestParameterized, cleanup )
{
SetUpAndActivateTrajectoryController( );
auto traj_node = traj_controller_->get_node( );
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node( traj_node->get_node_base_interface( ) );
// send msg
builtin_interfaces::msg::Duration time_from_start;
time_from_start.sec = 1;
time_from_start.nanosec = 0;
std::vector<std::vector<double>> points{{{3.3, 4.4, 5.5}}};
publish( time_from_start, points );
traj_controller_->wait_for_trajectory( executor );
traj_controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
auto state = traj_controller_->get_node( )->deactivate( );
ASSERT_EQ( State::PRIMARY_STATE_INACTIVE, state.id( ) );
traj_controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
state = traj_controller_->get_node( )->cleanup( );
ASSERT_EQ( State::PRIMARY_STATE_UNCONFIGURED, state.id( ) );
// update for 0.25 seconds
const auto start_time = rclcpp::Clock( ).now( );
updateController( rclcpp::Duration::from_seconds( 0.25 ) );
// should be home pose again
EXPECT_NEAR( INITIAL_POS_JOINT1, joint_pos_[0], COMMON_THRESHOLD );
EXPECT_NEAR( INITIAL_POS_JOINT2, joint_pos_[1], COMMON_THRESHOLD );
EXPECT_NEAR( INITIAL_POS_JOINT3, joint_pos_[2], COMMON_THRESHOLD );
executor.cancel( );
}
266 TEST_P( TrajectoryControllerTestParameterized, correct_initialization_using_parameters )
{
SetUpTrajectoryController( false );
// This call is replacing the way parameters are set via launch
SetParameters( );
SetParameters( ); // This call is replacing the way parameters are set via launch
traj_controller_->configure( );
auto state = traj_controller_->get_state( );
ASSERT_EQ( State::PRIMARY_STATE_INACTIVE, state.id( ) );
ActivateTrajectoryController( );
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node( traj_controller_->get_node( )->get_node_base_interface( ) );
state = traj_controller_->get_state( );
ASSERT_EQ( State::PRIMARY_STATE_ACTIVE, state.id( ) );
EXPECT_EQ( INITIAL_POS_JOINT1, joint_pos_[0] );
EXPECT_EQ( INITIAL_POS_JOINT2, joint_pos_[1] );
EXPECT_EQ( INITIAL_POS_JOINT3, joint_pos_[2] );
// send msg
constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds( 250 );
builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration( FIRST_POINT_TIME )};
// *INDENT-OFF*
std::vector<std::vector<double>> points{
{{3.3, 4.4, 5.5}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}};
std::vector<std::vector<double>> points_velocities{
{{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}};
// *INDENT-ON*
publish( time_from_start, points, rclcpp::Time( ), {}, points_velocities );
traj_controller_->wait_for_trajectory( executor );
// first update
traj_controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
// wait so controller process the second point when deactivated
std::this_thread::sleep_for( FIRST_POINT_TIME );
traj_controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
// deactivated
state = traj_controller_->get_node( )->deactivate( );
ASSERT_EQ( state.id( ), State::PRIMARY_STATE_INACTIVE );
// TODO( denis ): on my laptop I get delta of approx 0.1083. Is this me or is it something wrong?
// Come the flackiness here?
const auto allowed_delta = 0.11; // 0.05;
if ( traj_controller_->has_position_command_interface( ) )
{
EXPECT_NEAR( 3.3, joint_pos_[0], allowed_delta );
EXPECT_NEAR( 4.4, joint_pos_[1], allowed_delta );
EXPECT_NEAR( 5.5, joint_pos_[2], allowed_delta );
}
if ( traj_controller_->has_velocity_command_interface( ) )
{
EXPECT_LE( 0.0, joint_vel_[0] );
EXPECT_LE( 0.0, joint_vel_[1] );
EXPECT_LE( 0.0, joint_vel_[2] );
}
if ( traj_controller_->has_effort_command_interface( ) )
{
EXPECT_LE( 0.0, joint_eff_[0] );
EXPECT_LE( 0.0, joint_eff_[1] );
EXPECT_LE( 0.0, joint_eff_[2] );
}
// cleanup
state = traj_controller_->get_node( )->cleanup( );
// update loop receives a new msg and updates accordingly
traj_controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
// check the traj_msg_home_ptr_ initialization code for the standard wait timing
std::this_thread::sleep_for( std::chrono::milliseconds( 50 ) );
traj_controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
ASSERT_EQ( State::PRIMARY_STATE_UNCONFIGURED, state.id( ) );
EXPECT_NEAR( INITIAL_POS_JOINT1, joint_pos_[0], allowed_delta );
EXPECT_NEAR( INITIAL_POS_JOINT2, joint_pos_[1], allowed_delta );
EXPECT_NEAR( INITIAL_POS_JOINT3, joint_pos_[2], allowed_delta );
state = traj_controller_->get_node( )->configure( );
ASSERT_EQ( State::PRIMARY_STATE_INACTIVE, state.id( ) );
executor.cancel( );
}
354 TEST_P( TrajectoryControllerTestParameterized, state_topic_consistency )
{
rclcpp::executors::SingleThreadedExecutor executor;
SetUpAndActivateTrajectoryController( true, {}, &executor );
subscribeToState( );
updateController( );
// Spin to receive latest state
executor.spin_some( );
auto state = getState( );
size_t n_joints = joint_names_.size( );
for ( unsigned int i = 0; i < n_joints; ++i )
{
EXPECT_EQ( joint_names_[i], state->joint_names[i] );
}
// No trajectory by default, no desired state or error
EXPECT_TRUE( state->desired.positions.empty( ) );
EXPECT_TRUE( state->desired.velocities.empty( ) );
EXPECT_TRUE( state->desired.accelerations.empty( ) );
EXPECT_EQ( n_joints, state->actual.positions.size( ) );
if (
std::find( state_interface_types_.begin( ), state_interface_types_.end( ), "velocity" ) ==
state_interface_types_.end( ) )
{
EXPECT_TRUE( state->actual.velocities.empty( ) );
}
else
{
EXPECT_EQ( n_joints, state->actual.velocities.size( ) );
}
if (
std::find( state_interface_types_.begin( ), state_interface_types_.end( ), "acceleration" ) ==
state_interface_types_.end( ) )
{
EXPECT_TRUE( state->actual.accelerations.empty( ) );
}
else
{
EXPECT_EQ( n_joints, state->actual.accelerations.size( ) );
}
EXPECT_TRUE( state->error.positions.empty( ) );
EXPECT_TRUE( state->error.velocities.empty( ) );
EXPECT_TRUE( state->error.accelerations.empty( ) );
}
404 void TrajectoryControllerTest::test_state_publish_rate_target( int target_msg_count )
{
rclcpp::Parameter state_publish_rate_param(
"state_publish_rate", static_cast<double>( target_msg_count ) );
rclcpp::executors::SingleThreadedExecutor executor;
SetUpAndActivateTrajectoryController( true, {state_publish_rate_param}, &executor );
auto future_handle = std::async( std::launch::async, [&executor]( ) -> void { executor.spin( ); } );
using control_msgs::msg::JointTrajectoryControllerState;
const int qos_level = 10;
int echo_received_counter = 0;
rclcpp::Subscription<JointTrajectoryControllerState>::SharedPtr subs =
traj_controller_->get_node( )->create_subscription<JointTrajectoryControllerState>(
controller_name_ + "/state", qos_level,
[&]( JointTrajectoryControllerState::UniquePtr ) { ++echo_received_counter; } );
// update for 1second
const auto start_time = rclcpp::Clock( ).now( );
const rclcpp::Duration wait = rclcpp::Duration::from_seconds( 1.0 );
const auto end_time = start_time + wait;
while ( rclcpp::Clock( ).now( ) < end_time )
{
traj_controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
}
// We may miss the last message since time allowed is exactly the time needed
EXPECT_NEAR( target_msg_count, echo_received_counter, 1 );
executor.cancel( );
}
/**
* @brief test_state_publish_rate Test that state publish rate matches configure rate
*/
440 TEST_P( TrajectoryControllerTestParameterized, test_state_publish_rate )
{
test_state_publish_rate_target( 10 );
}
445 TEST_P( TrajectoryControllerTestParameterized, zero_state_publish_rate )
{
test_state_publish_rate_target( 0 );
}
/**
* @brief test_jumbled_joint_order Test sending trajectories with a joint order different from internal controller order
*/
453 TEST_P( TrajectoryControllerTestParameterized, test_jumbled_joint_order )
{
rclcpp::executors::SingleThreadedExecutor executor;
SetUpAndActivateTrajectoryController( true, {}, &executor );
{
trajectory_msgs::msg::JointTrajectory traj_msg;
const std::vector<std::string> jumbled_joint_names{
joint_names_[1], joint_names_[2], joint_names_[0]};
traj_msg.joint_names = jumbled_joint_names;
traj_msg.header.stamp = rclcpp::Time( 0 );
traj_msg.points.resize( 1 );
traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds( 0.25 );
traj_msg.points[0].positions.resize( 3 );
traj_msg.points[0].positions[0] = 2.0;
traj_msg.points[0].positions[1] = 3.0;
traj_msg.points[0].positions[2] = 1.0;
if ( traj_controller_->has_velocity_command_interface( ) )
{
traj_msg.points[0].velocities.resize( 3 );
traj_msg.points[0].velocities[0] = -0.1;
traj_msg.points[0].velocities[1] = -0.1;
traj_msg.points[0].velocities[2] = -0.1;
}
if ( traj_controller_->has_effort_command_interface( ) )
{
traj_msg.points[0].effort.resize( 3 );
traj_msg.points[0].effort[0] = -0.1;
traj_msg.points[0].effort[1] = -0.1;
traj_msg.points[0].effort[2] = -0.1;
}
trajectory_publisher_->publish( traj_msg );
}
traj_controller_->wait_for_trajectory( executor );
// update for 0.25 seconds
// TODO( destogl ): Make this time a bit shorter to increase stability on the CI?
// Currently COMMON_THRESHOLD is adjusted.
updateController( rclcpp::Duration::from_seconds( 0.25 ) );
if ( traj_controller_->has_position_command_interface( ) )
{
EXPECT_NEAR( 1.0, joint_pos_[0], COMMON_THRESHOLD );
EXPECT_NEAR( 2.0, joint_pos_[1], COMMON_THRESHOLD );
EXPECT_NEAR( 3.0, joint_pos_[2], COMMON_THRESHOLD );
}
if ( traj_controller_->has_velocity_command_interface( ) )
{
EXPECT_GT( 0.0, joint_vel_[0] );
EXPECT_GT( 0.0, joint_vel_[1] );
EXPECT_GT( 0.0, joint_vel_[2] );
}
if ( traj_controller_->has_effort_command_interface( ) )
{
EXPECT_GT( 0.0, joint_eff_[0] );
EXPECT_GT( 0.0, joint_eff_[1] );
EXPECT_GT( 0.0, joint_eff_[2] );
}
}
/**
* @brief test_partial_joint_list Test sending trajectories with a subset of the controlled joints
*/
521 TEST_P( TrajectoryControllerTestParameterized, test_partial_joint_list )
{
rclcpp::Parameter partial_joints_parameters( "allow_partial_joints_goal", true );
rclcpp::executors::SingleThreadedExecutor executor;
SetUpAndActivateTrajectoryController( true, {partial_joints_parameters}, &executor );
const double initial_joint1_cmd = joint_pos_[0];
const double initial_joint2_cmd = joint_pos_[1];
const double initial_joint3_cmd = joint_pos_[2];
trajectory_msgs::msg::JointTrajectory traj_msg;
{
std::vector<std::string> partial_joint_names{joint_names_[1], joint_names_[0]};
traj_msg.joint_names = partial_joint_names;
traj_msg.header.stamp = rclcpp::Time( 0 );
traj_msg.points.resize( 1 );
traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds( 0.25 );
traj_msg.points[0].positions.resize( 2 );
traj_msg.points[0].positions[0] = 2.0;
traj_msg.points[0].positions[1] = 1.0;
traj_msg.points[0].velocities.resize( 2 );
traj_msg.points[0].velocities[0] =
copysign( 2.0, traj_msg.points[0].positions[0] - initial_joint2_cmd );
traj_msg.points[0].velocities[1] =
copysign( 1.0, traj_msg.points[0].positions[1] - initial_joint1_cmd );
trajectory_publisher_->publish( traj_msg );
}
traj_controller_->wait_for_trajectory( executor );
// update for 0.5 seconds
updateController( rclcpp::Duration::from_seconds( 0.25 ) );
double threshold = 0.001;
if ( traj_controller_->has_position_command_interface( ) )
{
EXPECT_NEAR( traj_msg.points[0].positions[1], joint_pos_[0], threshold );
EXPECT_NEAR( traj_msg.points[0].positions[0], joint_pos_[1], threshold );
EXPECT_NEAR( initial_joint3_cmd, joint_pos_[2], threshold )
<< "Joint 3 command should be current position";
}
if (
std::find( command_interface_types_.begin( ), command_interface_types_.end( ), "velocity" ) !=
command_interface_types_.end( ) )
{
// estimate the sign of the velocity
// joint rotates forward
EXPECT_TRUE( is_same_sign( traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0] ) );
EXPECT_TRUE( is_same_sign( traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1] ) );
EXPECT_NEAR( 0.0, joint_vel_[2], threshold )
<< "Joint 3 velocity should be 0.0 since it's not in the goal";
}
if (
std::find( command_interface_types_.begin( ), command_interface_types_.end( ), "effort" ) !=
command_interface_types_.end( ) )
{
// estimate the sign of the velocity
// joint rotates forward
EXPECT_TRUE( is_same_sign( traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0] ) );
EXPECT_TRUE( is_same_sign( traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1] ) );
EXPECT_NEAR( 0.0, joint_eff_[2], threshold )
<< "Joint 3 effort should be 0.0 since it's not in the goal";
}
// TODO( anyone ): add here ckecks for acceleration commands
executor.cancel( );
}
/**
* @brief test_partial_joint_list Test sending trajectories with a subset of the controlled joints without allow_partial_joints_goal
*/
597 TEST_P( TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowed )
{
rclcpp::Parameter partial_joints_parameters( "allow_partial_joints_goal", false );
rclcpp::executors::SingleThreadedExecutor executor;
SetUpAndActivateTrajectoryController( true, {partial_joints_parameters}, &executor );
const double initial_joint1_cmd = joint_pos_[0];
const double initial_joint2_cmd = joint_pos_[1];
const double initial_joint3_cmd = joint_pos_[2];
const double initial_joint_vel = 0.0;
const double initial_joint_acc = 0.0;
trajectory_msgs::msg::JointTrajectory traj_msg;
{
std::vector<std::string> partial_joint_names{joint_names_[1], joint_names_[0]};
traj_msg.joint_names = partial_joint_names;
traj_msg.header.stamp = rclcpp::Time( 0 );
traj_msg.points.resize( 1 );
traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds( 0.25 );
traj_msg.points[0].positions.resize( 2 );
traj_msg.points[0].positions[0] = 2.0;
traj_msg.points[0].positions[1] = 1.0;
traj_msg.points[0].velocities.resize( 2 );
traj_msg.points[0].velocities[0] = 2.0;
traj_msg.points[0].velocities[1] = 1.0;
trajectory_publisher_->publish( traj_msg );
}
traj_controller_->wait_for_trajectory( executor );
// update for 0.5 seconds
updateController( rclcpp::Duration::from_seconds( 0.25 ) );
double threshold = 0.001;
EXPECT_NEAR( initial_joint1_cmd, joint_pos_[0], threshold )
<< "All joints command should be current position because goal was rejected";
EXPECT_NEAR( initial_joint2_cmd, joint_pos_[1], threshold )
<< "All joints command should be current position because goal was rejected";
EXPECT_NEAR( initial_joint3_cmd, joint_pos_[2], threshold )
<< "All joints command should be current position because goal was rejected";
if (
std::find( command_interface_types_.begin( ), command_interface_types_.end( ), "velocity" ) !=
command_interface_types_.end( ) )
{
EXPECT_NEAR( initial_joint_vel, joint_vel_[0], threshold )
<< "All joints velocities should be 0.0 because goal was rejected";
EXPECT_NEAR( initial_joint_vel, joint_vel_[1], threshold )
<< "All joints velocities should be 0.0 because goal was rejected";
EXPECT_NEAR( initial_joint_vel, joint_vel_[2], threshold )
<< "All joints velocities should be 0.0 because goal was rejected";
}
if (
std::find( command_interface_types_.begin( ), command_interface_types_.end( ), "acceleration" ) !=
command_interface_types_.end( ) )
{
EXPECT_NEAR( initial_joint_acc, joint_acc_[0], threshold )
<< "All joints accelerations should be 0.0 because goal was rejected";
EXPECT_NEAR( initial_joint_acc, joint_acc_[1], threshold )
<< "All joints accelerations should be 0.0 because goal was rejected";
EXPECT_NEAR( initial_joint_acc, joint_acc_[2], threshold )
<< "All joints accelerations should be 0.0 because goal was rejected";
}
executor.cancel( );
}
/**
* @brief invalid_message Test mismatched joint and reference vector sizes
*/
670 TEST_P( TrajectoryControllerTestParameterized, invalid_message )
{
rclcpp::Parameter partial_joints_parameters( "allow_partial_joints_goal", false );
rclcpp::Parameter allow_integration_parameters( "allow_integration_in_goal_trajectories", false );
rclcpp::executors::SingleThreadedExecutor executor;
SetUpAndActivateTrajectoryController(
true, {partial_joints_parameters, allow_integration_parameters}, &executor );
trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg;
good_traj_msg.joint_names = joint_names_;
good_traj_msg.header.stamp = rclcpp::Time( 0 );
good_traj_msg.points.resize( 1 );
good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds( 0.25 );
good_traj_msg.points[0].positions.resize( 1 );
good_traj_msg.points[0].positions = {1.0, 2.0, 3.0};
good_traj_msg.points[0].velocities.resize( 1 );
good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0};
EXPECT_TRUE( traj_controller_->validate_trajectory_msg( good_traj_msg ) );
// Incompatible joint names
traj_msg = good_traj_msg;
traj_msg.joint_names = {"bad_name"};
EXPECT_FALSE( traj_controller_->validate_trajectory_msg( traj_msg ) );
// No position data
traj_msg = good_traj_msg;
traj_msg.points[0].positions.clear( );
EXPECT_FALSE( traj_controller_->validate_trajectory_msg( traj_msg ) );
// Incompatible data sizes, too few positions
traj_msg = good_traj_msg;
traj_msg.points[0].positions = {1.0, 2.0};
EXPECT_FALSE( traj_controller_->validate_trajectory_msg( traj_msg ) );
// Incompatible data sizes, too many positions
traj_msg = good_traj_msg;
traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0};
EXPECT_FALSE( traj_controller_->validate_trajectory_msg( traj_msg ) );
// Incompatible data sizes, too few velocities
traj_msg = good_traj_msg;
traj_msg.points[0].velocities = {1.0, 2.0};
EXPECT_FALSE( traj_controller_->validate_trajectory_msg( traj_msg ) );
// Incompatible data sizes, too few accelerations
traj_msg = good_traj_msg;
traj_msg.points[0].accelerations = {1.0, 2.0};
EXPECT_FALSE( traj_controller_->validate_trajectory_msg( traj_msg ) );
// Incompatible data sizes, too few efforts
traj_msg = good_traj_msg;
traj_msg.points[0].positions.clear( );
traj_msg.points[0].effort = {1.0, 2.0};
EXPECT_FALSE( traj_controller_->validate_trajectory_msg( traj_msg ) );
// Non-strictly increasing waypoint times
traj_msg = good_traj_msg;
traj_msg.points.push_back( traj_msg.points.front( ) );
EXPECT_FALSE( traj_controller_->validate_trajectory_msg( traj_msg ) );
}
/// With allow_integration_in_goal_trajectories parameter trajectory missing position or velocities
/// are accepted
734 TEST_P( TrajectoryControllerTestParameterized, missing_positions_message_accepted )
{
rclcpp::Parameter allow_integration_parameters( "allow_integration_in_goal_trajectories", true );
rclcpp::executors::SingleThreadedExecutor executor;
SetUpAndActivateTrajectoryController( true, {allow_integration_parameters}, &executor );
trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg;
good_traj_msg.joint_names = joint_names_;
good_traj_msg.header.stamp = rclcpp::Time( 0 );
good_traj_msg.points.resize( 1 );
good_traj_msg.points[0].time_from_start = rclcpp::Duration::from_seconds( 0.25 );
good_traj_msg.points[0].positions.resize( 1 );
good_traj_msg.points[0].positions = {1.0, 2.0, 3.0};
good_traj_msg.points[0].velocities.resize( 1 );
good_traj_msg.points[0].velocities = {-1.0, -2.0, -3.0};
good_traj_msg.points[0].accelerations.resize( 1 );
good_traj_msg.points[0].accelerations = {1.0, 2.0, 3.0};
EXPECT_TRUE( traj_controller_->validate_trajectory_msg( good_traj_msg ) );
// No position data
traj_msg = good_traj_msg;
traj_msg.points[0].positions.clear( );
EXPECT_TRUE( traj_controller_->validate_trajectory_msg( traj_msg ) );
// No position and velocity data
traj_msg = good_traj_msg;
traj_msg.points[0].positions.clear( );
traj_msg.points[0].velocities.clear( );
EXPECT_TRUE( traj_controller_->validate_trajectory_msg( traj_msg ) );
// All empty
traj_msg = good_traj_msg;
traj_msg.points[0].positions.clear( );
traj_msg.points[0].velocities.clear( );
traj_msg.points[0].accelerations.clear( );
EXPECT_FALSE( traj_controller_->validate_trajectory_msg( traj_msg ) );
// Incompatible data sizes, too few positions
traj_msg = good_traj_msg;
traj_msg.points[0].positions = {1.0, 2.0};
EXPECT_FALSE( traj_controller_->validate_trajectory_msg( traj_msg ) );
// Incompatible data sizes, too many positions
traj_msg = good_traj_msg;
traj_msg.points[0].positions = {1.0, 2.0, 3.0, 4.0};
EXPECT_FALSE( traj_controller_->validate_trajectory_msg( traj_msg ) );
// Incompatible data sizes, too few velocities
traj_msg = good_traj_msg;
traj_msg.points[0].velocities = {1.0};
EXPECT_FALSE( traj_controller_->validate_trajectory_msg( traj_msg ) );
// Incompatible data sizes, too few accelerations
traj_msg = good_traj_msg;
traj_msg.points[0].accelerations = {2.0};
EXPECT_FALSE( traj_controller_->validate_trajectory_msg( traj_msg ) );
}
/**
* @brief test_trajectory_replace Test replacing an existing trajectory
*/
796 TEST_P( TrajectoryControllerTestParameterized, test_trajectory_replace )
{
rclcpp::executors::SingleThreadedExecutor executor;
rclcpp::Parameter partial_joints_parameters( "allow_partial_joints_goal", true );
SetUpAndActivateTrajectoryController( true, {partial_joints_parameters}, &executor );
subscribeToState( );
std::vector<std::vector<double>> points_old{{{2., 3., 4.}}};
std::vector<std::vector<double>> points_old_velocities{{{0.2, 0.3, 0.4}}};
std::vector<std::vector<double>> points_partial_new{{1.5}};
std::vector<std::vector<double>> points_partial_new_velocities{{0.15}};
const auto delay = std::chrono::milliseconds( 500 );
builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration( delay )};
publish( time_from_start, points_old, rclcpp::Time( ), {}, points_old_velocities );
trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired;
expected_actual.positions = {points_old[0].begin( ), points_old[0].end( )};
expected_desired.positions = {points_old[0].begin( ), points_old[0].end( )};
expected_actual.velocities = {points_old_velocities[0].begin( ), points_old_velocities[0].end( )};
expected_desired.velocities = {points_old_velocities[0].begin( ), points_old_velocities[0].end( )};
// Check that we reached end of points_old trajectory
// Denis: delta was 0.1 with 0.2 works for me
waitAndCompareState( expected_actual, expected_desired, executor, rclcpp::Duration( delay ), 0.2 );
RCLCPP_INFO( traj_controller_->get_node( )->get_logger( ), "Sending new trajectory" );
points_partial_new_velocities[0][0] =
copysign( 0.15, points_partial_new[0][0] - joint_state_pos_[0] );
publish( time_from_start, points_partial_new, rclcpp::Time( ), {}, points_partial_new_velocities );
// Replaced trajectory is a mix of previous and current goal
expected_desired.positions[0] = points_partial_new[0][0];
expected_desired.positions[1] = points_old[0][1];
expected_desired.positions[2] = points_old[0][2];
expected_desired.velocities[0] = points_partial_new_velocities[0][0];
expected_desired.velocities[1] = 0.0;
expected_desired.velocities[2] = 0.0;
expected_actual = expected_desired;
waitAndCompareState( expected_actual, expected_desired, executor, rclcpp::Duration( delay ), 0.1 );
}
/**
* @brief test_ignore_old_trajectory Sending an old trajectory replacing an existing trajectory
*/
840 TEST_P( TrajectoryControllerTestParameterized, test_ignore_old_trajectory )
{
rclcpp::executors::SingleThreadedExecutor executor;
SetUpAndActivateTrajectoryController( true, {}, &executor );
subscribeToState( );
// TODO( anyone ): add expectations for velocities and accelerations
std::vector<std::vector<double>> points_old{{{2., 3., 4.}, {4., 5., 6.}}};
std::vector<std::vector<double>> points_new{{{-1., -2., -3.}}};
const auto delay = std::chrono::milliseconds( 500 );
builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration( delay )};
publish( time_from_start, points_old );
trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired;
expected_actual.positions = {points_old[0].begin( ), points_old[0].end( )};
expected_desired = expected_actual;
// Check that we reached end of points_old[0] trajectory
waitAndCompareState( expected_actual, expected_desired, executor, rclcpp::Duration( delay ), 0.1 );
RCLCPP_INFO( traj_controller_->get_node( )->get_logger( ), "Sending new trajectory in the past" );
// New trajectory will end before current time
rclcpp::Time new_traj_start = rclcpp::Clock( ).now( ) - delay - std::chrono::milliseconds( 100 );
expected_actual.positions = {points_old[1].begin( ), points_old[1].end( )};
expected_desired = expected_actual;
publish( time_from_start, points_new, new_traj_start );
waitAndCompareState( expected_actual, expected_desired, executor, rclcpp::Duration( delay ), 0.1 );
}
868 TEST_P( TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory )
{
rclcpp::executors::SingleThreadedExecutor executor;
SetUpAndActivateTrajectoryController( true, {}, &executor );
subscribeToState( );
std::vector<std::vector<double>> points_old{{{2., 3., 4.}, {4., 5., 6.}}};
std::vector<std::vector<double>> points_new{{{-1., -2., -3.}, {-2., -4., -6.}}};
const auto delay = std::chrono::milliseconds( 500 );
builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration( delay )};
publish( time_from_start, points_old );
trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired;
expected_actual.positions = {points_old[0].begin( ), points_old[0].end( )};
expected_desired = expected_actual;
// Check that we reached end of points_old[0]trajectory
waitAndCompareState( expected_actual, expected_desired, executor, rclcpp::Duration( delay ), 0.1 );
RCLCPP_INFO(
traj_controller_->get_node( )->get_logger( ), "Sending new trajectory partially in the past" );
// New trajectory first point is in the past, second is in the future
rclcpp::Time new_traj_start = rclcpp::Clock( ).now( ) - delay - std::chrono::milliseconds( 100 );
expected_actual.positions = {points_new[1].begin( ), points_new[1].end( )};
expected_desired = expected_actual;
publish( time_from_start, points_new, new_traj_start );
waitAndCompareState( expected_actual, expected_desired, executor, rclcpp::Duration( delay ), 0.1 );
}
896 TEST_P( TrajectoryControllerTestParameterized, test_execute_partial_traj_in_future )
{
SetUpTrajectoryController( );
auto traj_node = traj_controller_->get_node( );
RCLCPP_WARN(
traj_node->get_logger( ),
"Test disabled until current_trajectory is taken into account when adding a new trajectory." );
// https://github.com/ros-controls/ros_controllers/blob/melodic-devel/joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h#L149
return;
// TODO( anyone ): use SetUpAndActivateTrajectoryController method instead of the next line
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node( traj_node->get_node_base_interface( ) );
subscribeToState( );
rclcpp::Parameter partial_joints_parameters( "allow_partial_joints_goal", true );
traj_node->set_parameter( partial_joints_parameters );
traj_controller_->get_node( )->configure( );
traj_controller_->get_node( )->activate( );
std::vector<std::vector<double>> full_traj{{{2., 3., 4.}, {4., 6., 8.}}};
std::vector<std::vector<double>> full_traj_velocities{{{0.2, 0.3, 0.4}, {0.4, 0.6, 0.8}}};
std::vector<std::vector<double>> partial_traj{
{{-1., -2.},
{
-2.,
-4,
}}};
std::vector<std::vector<double>> partial_traj_velocities{
{{-0.1, -0.2},
{
-0.2,
-0.4,
}}};
const auto delay = std::chrono::milliseconds( 500 );
builtin_interfaces::msg::Duration points_delay{rclcpp::Duration( delay )};
// Send full trajectory
publish( points_delay, full_traj, rclcpp::Time( ), {}, full_traj_velocities );
// Sleep until first waypoint of full trajectory
trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired;
expected_actual.positions = {full_traj[0].begin( ), full_traj[0].end( )};
expected_desired = expected_actual;
// Check that we reached end of points_old[0]trajectory and are starting points_old[1]
waitAndCompareState( expected_actual, expected_desired, executor, rclcpp::Duration( delay ), 0.1 );
// Send partial trajectory starting after full trajecotry is complete
RCLCPP_INFO( traj_node->get_logger( ), "Sending new trajectory in the future" );
publish(
points_delay, partial_traj, rclcpp::Clock( ).now( ) + delay * 2, {}, partial_traj_velocities );
// Wait until the end start and end of partial traj
expected_actual.positions = {partial_traj.back( )[0], partial_traj.back( )[1], full_traj.back( )[2]};
expected_desired = expected_actual;
waitAndCompareState(
expected_actual, expected_desired, executor, rclcpp::Duration( delay * ( 2 + 2 ) ), 0.1 );
}
954 TEST_P( TrajectoryControllerTestParameterized, test_jump_when_state_tracking_error_updated )
{
rclcpp::executors::SingleThreadedExecutor executor;
// default if false so it will not be actually set parameter
rclcpp::Parameter is_open_loop_parameters( "open_loop_control", false );
SetUpAndActivateTrajectoryController( true, {is_open_loop_parameters}, &executor, true );
// goal setup
std::vector<double> first_goal = {3.3, 4.4, 5.5};
std::vector<std::vector<double>> first_goal_velocities = {{0.33, 0.44, 0.55}};
std::vector<double> second_goal = {6.6, 8.8, 11.0};
std::vector<std::vector<double>> second_goal_velocities = {{0.66, 0.88, 1.1}};
double state_from_command_offset = 0.3;
// send msg
builtin_interfaces::msg::Duration time_from_start;
time_from_start.sec = 1;
time_from_start.nanosec = 0;
std::vector<std::vector<double>> points{{first_goal}};
publish( time_from_start, points, rclcpp::Time( ), {}, first_goal_velocities );
traj_controller_->wait_for_trajectory( executor );
updateController( rclcpp::Duration::from_seconds( 1.1 ) );
if ( traj_controller_->has_position_command_interface( ) )
{
// JTC is executing trajectory in open-loop therefore:
// - internal state does not have to be updated ( in this test-case it shouldn't )
// - internal command is updated
EXPECT_NEAR( INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD );
EXPECT_NEAR( first_goal[0], joint_pos_[0], COMMON_THRESHOLD );
// State interface should have offset from the command before starting a new trajectory
joint_state_pos_[0] = first_goal[0] - state_from_command_offset;
// Move joint further in the same direction as before ( to the second goal )
points = {{second_goal}};
publish( time_from_start, points, rclcpp::Time( ), {}, second_goal_velocities );
traj_controller_->wait_for_trajectory( executor );
// One the first update( s ) there should be a "jump" in opposite direction from command
// ( towards the state value )
EXPECT_NEAR( first_goal[0], joint_pos_[0], COMMON_THRESHOLD );
traj_controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
// Expect backward commands at first
EXPECT_NEAR( joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD );
EXPECT_GT( joint_pos_[0], joint_state_pos_[0] );
EXPECT_LT( joint_pos_[0], first_goal[0] );
traj_controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
EXPECT_GT( joint_pos_[0], joint_state_pos_[0] );
EXPECT_LT( joint_pos_[0], first_goal[0] );
traj_controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
EXPECT_GT( joint_pos_[0], joint_state_pos_[0] );
EXPECT_LT( joint_pos_[0], first_goal[0] );
// Finally the second goal will be commanded/reached
updateController( rclcpp::Duration::from_seconds( 1.1 ) );
EXPECT_NEAR( second_goal[0], joint_pos_[0], COMMON_THRESHOLD );
// State interface should have offset from the command before starting a new trajectory
joint_state_pos_[0] = second_goal[0] - state_from_command_offset;
// Move joint back to the first goal
points = {{first_goal}};
publish( time_from_start, points );
traj_controller_->wait_for_trajectory( executor );
// One the first update( s ) there should be a "jump" in the goal direction from command
// ( towards the state value )
EXPECT_NEAR( second_goal[0], joint_pos_[0], COMMON_THRESHOLD );
traj_controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
// Expect backward commands at first
EXPECT_NEAR( joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD );
EXPECT_LT( joint_pos_[0], joint_state_pos_[0] );
EXPECT_GT( joint_pos_[0], first_goal[0] );
traj_controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
EXPECT_LT( joint_pos_[0], joint_state_pos_[0] );
EXPECT_GT( joint_pos_[0], first_goal[0] );
traj_controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
EXPECT_LT( joint_pos_[0], joint_state_pos_[0] );
EXPECT_GT( joint_pos_[0], first_goal[0] );
// Finally the first goal will be commanded/reached
updateController( rclcpp::Duration::from_seconds( 1.1 ) );
EXPECT_NEAR( first_goal[0], joint_pos_[0], COMMON_THRESHOLD );
}
executor.cancel( );
}
1043 TEST_P( TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_error_not_updated )
{
rclcpp::executors::SingleThreadedExecutor executor;
// default if false so it will not be actually set parameter
rclcpp::Parameter is_open_loop_parameters( "open_loop_control", true );
SetUpAndActivateTrajectoryController( true, {is_open_loop_parameters}, &executor, true );
// goal setup
std::vector<double> first_goal = {3.3, 4.4, 5.5};
std::vector<double> second_goal = {6.6, 8.8, 11.0};
double state_from_command_offset = 0.3;
// send msg
builtin_interfaces::msg::Duration time_from_start;
time_from_start.sec = 1;
time_from_start.nanosec = 0;
std::vector<std::vector<double>> points{{first_goal}};
publish( time_from_start, points );
traj_controller_->wait_for_trajectory( executor );
updateController( rclcpp::Duration::from_seconds( 1.1 ) );
if ( traj_controller_->has_position_command_interface( ) )
{
// JTC is executing trajectory in open-loop therefore:
// - internal state does not have to be updated ( in this test-case it shouldn't )
// - internal command is updated
EXPECT_NEAR( INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD );
EXPECT_NEAR( first_goal[0], joint_pos_[0], COMMON_THRESHOLD );
// State interface should have offset from the command before starting a new trajectory
joint_state_pos_[0] = first_goal[0] - state_from_command_offset;
// Move joint further in the same direction as before ( to the second goal )
points = {{second_goal}};
publish( time_from_start, points );
traj_controller_->wait_for_trajectory( executor );
// One the first update( s ) there **should not** be a "jump" in opposite direction from command
// ( towards the state value )
EXPECT_NEAR( first_goal[0], joint_pos_[0], COMMON_THRESHOLD );
traj_controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
// There should not be backward commands
EXPECT_NEAR( first_goal[0], joint_pos_[0], COMMON_THRESHOLD );
EXPECT_GT( joint_pos_[0], first_goal[0] );
EXPECT_LT( joint_pos_[0], second_goal[0] );
traj_controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
EXPECT_GT( joint_pos_[0], first_goal[0] );
EXPECT_LT( joint_pos_[0], second_goal[0] );
traj_controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
EXPECT_GT( joint_pos_[0], first_goal[0] );
EXPECT_LT( joint_pos_[0], second_goal[0] );
// Finally the second goal will be commanded/reached
updateController( rclcpp::Duration::from_seconds( 1.1 ) );
EXPECT_NEAR( second_goal[0], joint_pos_[0], COMMON_THRESHOLD );
// State interface should have offset from the command before starting a new trajectory
joint_state_pos_[0] = second_goal[0] - state_from_command_offset;
// Move joint back to the first goal
points = {{first_goal}};
publish( time_from_start, points );
traj_controller_->wait_for_trajectory( executor );
// One the first update( s ) there **should not** be a "jump" in the goal direction from command
// ( towards the state value )
EXPECT_NEAR( second_goal[0], joint_pos_[0], COMMON_THRESHOLD );
traj_controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
// There should not be a jump toward commands
EXPECT_NEAR( second_goal[0], joint_pos_[0], COMMON_THRESHOLD );
EXPECT_LT( joint_pos_[0], second_goal[0] );
EXPECT_GT( joint_pos_[0], first_goal[0] );
traj_controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
EXPECT_GT( joint_pos_[0], first_goal[0] );
EXPECT_LT( joint_pos_[0], second_goal[0] );
traj_controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) );
EXPECT_GT( joint_pos_[0], first_goal[0] );
EXPECT_LT( joint_pos_[0], second_goal[0] );
// Finally the first goal will be commanded/reached
updateController( rclcpp::Duration::from_seconds( 1.1 ) );
EXPECT_NEAR( first_goal[0], joint_pos_[0], COMMON_THRESHOLD );
}
executor.cancel( );
}
// Testing that values are read from state interfaces when hardware is started for the first
// time and hardware state has offset --> this is indicated by NaN values in state interfaces
1132 TEST_P( TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_controller_start )
{
rclcpp::executors::SingleThreadedExecutor executor;
// default if false so it will not be actually set parameter
rclcpp::Parameter is_open_loop_parameters( "open_loop_control", true );
// set command values to NaN
for ( size_t i = 0; i < 3; ++i )
{
joint_pos_[i] = std::numeric_limits<double>::quiet_NaN( );
joint_vel_[i] = std::numeric_limits<double>::quiet_NaN( );
joint_acc_[i] = std::numeric_limits<double>::quiet_NaN( );
}
SetUpAndActivateTrajectoryController( true, {is_open_loop_parameters}, &executor, true );
auto current_state_when_offset = traj_controller_->get_current_state_when_offset( );
for ( size_t i = 0; i < 3; ++i )
{
EXPECT_EQ( current_state_when_offset.positions[i], joint_state_pos_[i] );
// check velocity
if (
std::find(
state_interface_types_.begin( ), state_interface_types_.end( ),
hardware_interface::HW_IF_VELOCITY ) != state_interface_types_.end( ) &&
std::find(
command_interface_types_.begin( ), command_interface_types_.end( ),
hardware_interface::HW_IF_VELOCITY ) != command_interface_types_.end( ) )
{
EXPECT_EQ( current_state_when_offset.positions[i], joint_state_pos_[i] );
}
// check acceleration
if (
std::find(
state_interface_types_.begin( ), state_interface_types_.end( ),
hardware_interface::HW_IF_ACCELERATION ) != state_interface_types_.end( ) &&
std::find(
command_interface_types_.begin( ), command_interface_types_.end( ),
hardware_interface::HW_IF_ACCELERATION ) != command_interface_types_.end( ) )
{
EXPECT_EQ( current_state_when_offset.positions[i], joint_state_pos_[i] );
}
}
executor.cancel( );
}
// Testing that values are read from state interfaces when hardware is started after some values
// are set on the hardware commands
1183 TEST_P( TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_controller_start )
{
rclcpp::executors::SingleThreadedExecutor executor;
// default if false so it will not be actually set parameter
rclcpp::Parameter is_open_loop_parameters( "open_loop_control", true );
// set command values to NaN
for ( size_t i = 0; i < 3; ++i )
{
joint_pos_[i] = 3.1 + i;
joint_vel_[i] = 0.25 + i;
joint_acc_[i] = 0.02 + i / 10.0;
}
SetUpAndActivateTrajectoryController( true, {is_open_loop_parameters}, &executor, true );
auto current_state_when_offset = traj_controller_->get_current_state_when_offset( );
for ( size_t i = 0; i < 3; ++i )
{
EXPECT_EQ( current_state_when_offset.positions[i], joint_pos_[i] );
// check velocity
if (
std::find(
state_interface_types_.begin( ), state_interface_types_.end( ),
hardware_interface::HW_IF_VELOCITY ) != state_interface_types_.end( ) &&
std::find(
command_interface_types_.begin( ), command_interface_types_.end( ),
hardware_interface::HW_IF_VELOCITY ) != command_interface_types_.end( ) )
{
EXPECT_EQ( current_state_when_offset.positions[i], joint_pos_[i] );
}
// check acceleration
if (
std::find(
state_interface_types_.begin( ), state_interface_types_.end( ),
hardware_interface::HW_IF_ACCELERATION ) != state_interface_types_.end( ) &&
std::find(
command_interface_types_.begin( ), command_interface_types_.end( ),
hardware_interface::HW_IF_ACCELERATION ) != command_interface_types_.end( ) )
{
EXPECT_EQ( current_state_when_offset.positions[i], joint_pos_[i] );
}
}
executor.cancel( );
}
// TODO( andyz ): disabled because they started failing at the transition to Humble
/*
// position controllers
INSTANTIATE_TEST_SUITE_P(
PositionTrajectoryControllers, TrajectoryControllerTestParameterized,
::testing::Values(
std::make_tuple( std::vector<std::string>( {"position"} ), std::vector<std::string>( {"position"} ) ),
std::make_tuple(
std::vector<std::string>( {"position"} ), std::vector<std::string>( {"position", "velocity"} ) ),
std::make_tuple(
std::vector<std::string>( {"position"} ),
std::vector<std::string>( {"position", "velocity", "acceleration"} ) ) ) );
// position_velocity controllers
INSTANTIATE_TEST_SUITE_P(
PositionVelocityTrajectoryControllers, TrajectoryControllerTestParameterized,
::testing::Values(
std::make_tuple(
std::vector<std::string>( {"position", "velocity"} ), std::vector<std::string>( {"position"} ) ),
std::make_tuple(
std::vector<std::string>( {"position", "velocity"} ),
std::vector<std::string>( {"position", "velocity"} ) ),
std::make_tuple(
std::vector<std::string>( {"position", "velocity"} ),
std::vector<std::string>( {"position", "velocity", "acceleration"} ) ) ) );
// position_velocity_acceleration controllers
INSTANTIATE_TEST_SUITE_P(
PositionVelocityAccelerationTrajectoryControllers, TrajectoryControllerTestParameterized,
::testing::Values(
std::make_tuple(
std::vector<std::string>( {"position", "velocity", "acceleration"} ),
std::vector<std::string>( {"position"} ) ),
std::make_tuple(
std::vector<std::string>( {"position", "velocity", "acceleration"} ),
std::vector<std::string>( {"position", "velocity"} ) ),
std::make_tuple(
std::vector<std::string>( {"position", "velocity", "acceleration"} ),
std::vector<std::string>( {"position", "velocity", "acceleration"} ) ) ) );
// only velocity controller
INSTANTIATE_TEST_SUITE_P(
OnlyVelocityTrajectoryControllers, TrajectoryControllerTestParameterized,
::testing::Values(
std::make_tuple(
std::vector<std::string>( {"velocity"} ), std::vector<std::string>( {"position", "velocity"} ) ),
std::make_tuple(
std::vector<std::string>( {"velocity"} ),
std::vector<std::string>( {"position", "velocity", "acceleration"} ) ) ) );
// only effort controller
INSTANTIATE_TEST_SUITE_P(
OnlyEffortTrajectoryControllers, TrajectoryControllerTestParameterized,
::testing::Values(
std::make_tuple(
std::vector<std::string>( {"effort"} ), std::vector<std::string>( {"position", "velocity"} ) ),
std::make_tuple(
std::vector<std::string>( {"effort"} ),
std::vector<std::string>( {"position", "velocity", "acceleration"} ) ) ) );
*/
1293 TEST_F( TrajectoryControllerTest, incorrect_initialization_using_interface_parameters )
{
auto set_parameter_and_check_result = [&]( ) {
EXPECT_EQ( traj_controller_->get_state( ).id( ), State::PRIMARY_STATE_UNCONFIGURED );
SetParameters( ); // This call is replacing the way parameters are set via launch
traj_controller_->get_node( )->configure( );
EXPECT_EQ( traj_controller_->get_state( ).id( ), State::PRIMARY_STATE_UNCONFIGURED );
};
SetUpTrajectoryController( false );
// command interfaces: empty
command_interface_types_ = {};
set_parameter_and_check_result( );
// command interfaces: bad_name
command_interface_types_ = {"bad_name"};
set_parameter_and_check_result( );
// command interfaces: effort has to be only
command_interface_types_ = {"effort", "position"};
set_parameter_and_check_result( );
// command interfaces: velocity - position not present
command_interface_types_ = {"velocity", "acceleration"};
set_parameter_and_check_result( );
// command interfaces: acceleration without position and velocity
command_interface_types_ = {"acceleration"};
set_parameter_and_check_result( );
// state interfaces: empty
state_interface_types_ = {};
set_parameter_and_check_result( );
// state interfaces: cannot not be effort
state_interface_types_ = {"effort"};
set_parameter_and_check_result( );
// state interfaces: bad name
state_interface_types_ = {"bad_name"};
set_parameter_and_check_result( );
// state interfaces: velocity - position not present
state_interface_types_ = {"velocity"};
set_parameter_and_check_result( );
state_interface_types_ = {"velocity", "acceleration"};
set_parameter_and_check_result( );
// state interfaces: acceleration without position and velocity
state_interface_types_ = {"acceleration"};
set_parameter_and_check_result( );
// velocity-only command interface: position - velocity not present
command_interface_types_ = {"velocity"};
state_interface_types_ = {"position"};
set_parameter_and_check_result( );
state_interface_types_ = {"velocity"};
set_parameter_and_check_result( );
// effort-only command interface: position - velocity not present
command_interface_types_ = {"effort"};
state_interface_types_ = {"position"};
set_parameter_and_check_result( );
state_interface_types_ = {"velocity"};
set_parameter_and_check_result( );
}
// Copyright 2020 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TEST_TRAJECTORY_CONTROLLER_UTILS_HPP_
#define TEST_TRAJECTORY_CONTROLLER_UTILS_HPP_
#include <memory>
#include <string>
#include <tuple>
#include <utility>
#include <vector>
#include "gtest/gtest.h"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "joint_trajectory_controller/joint_trajectory_controller.hpp"
namespace
{
const double COMMON_THRESHOLD = 0.0011; // destogl: increased for 0.0001 for stable CI builds?
const double INITIAL_POS_JOINT1 = 1.1;
const double INITIAL_POS_JOINT2 = 2.1;
const double INITIAL_POS_JOINT3 = 3.1;
const std::vector<double> INITIAL_POS_JOINTS = {
INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3};
const std::vector<double> INITIAL_VEL_JOINTS = {0.0, 0.0, 0.0};
const std::vector<double> INITIAL_ACC_JOINTS = {0.0, 0.0, 0.0};
const std::vector<double> INITIAL_EFF_JOINTS = {0.0, 0.0, 0.0};
} // namespace
namespace test_trajectory_controllers
{
44 class TestableJointTrajectoryController
45 : public joint_trajectory_controller::JointTrajectoryController
{
public:
using joint_trajectory_controller::JointTrajectoryController::JointTrajectoryController;
using joint_trajectory_controller::JointTrajectoryController::validate_trajectory_msg;
51 controller_interface::CallbackReturn on_configure(
const rclcpp_lifecycle::State & previous_state ) override
{
auto ret = joint_trajectory_controller::JointTrajectoryController::on_configure( previous_state );
// this class can still be useful without the wait set
if ( joint_command_subscriber_ )
{
joint_cmd_sub_wait_set_.add_subscription( joint_command_subscriber_ );
}
return ret;
}
/**
* @brief wait_for_trajectory block until a new JointTrajectory is received.
* Requires that the executor is not spinned elsewhere between the
* message publication and the call to this function
*
* @return true if new JointTrajectory msg was received, false if timeout
*/
70 bool wait_for_trajectory(
rclcpp::Executor & executor,
const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500} )
{
bool success = joint_cmd_sub_wait_set_.wait( timeout ).kind( ) == rclcpp::WaitResultKind::Ready;
if ( success )
{
executor.spin_some( );
}
return success;
}
82 void set_joint_names( const std::vector<std::string> & joint_names ) { joint_names_ = joint_names; }
84 void set_command_interfaces( const std::vector<std::string> & command_interfaces )
{
command_interface_types_ = command_interfaces;
}
89 void set_state_interfaces( const std::vector<std::string> & state_interfaces )
{
state_interface_types_ = state_interfaces;
}
94 trajectory_msgs::msg::JointTrajectoryPoint get_current_state_when_offset( )
{
return last_commanded_state_;
}
99 bool has_position_command_interface( ) { return has_position_command_interface_; }
101 bool has_velocity_command_interface( ) { return has_velocity_command_interface_; }
103 bool has_effort_command_interface( ) { return has_effort_command_interface_; }
rclcpp::WaitSet joint_cmd_sub_wait_set_;
};
108 class TrajectoryControllerTest : public ::testing::Test
{
public:
111 static void SetUpTestCase( ) { rclcpp::init( 0, nullptr ); }
113 virtual void SetUp( )
{
controller_name_ = "test_joint_trajectory_controller";
joint_names_ = {"joint1", "joint2", "joint3"};
joint_pos_.resize( joint_names_.size( ), 0.0 );
joint_state_pos_.resize( joint_names_.size( ), 0.0 );
joint_vel_.resize( joint_names_.size( ), 0.0 );
joint_state_vel_.resize( joint_names_.size( ), 0.0 );
joint_acc_.resize( joint_names_.size( ), 0.0 );
joint_state_acc_.resize( joint_names_.size( ), 0.0 );
joint_eff_.resize( joint_names_.size( ), 0.0 );
// Default interface values - they will be overwritten by parameterized tests
command_interface_types_ = {"position"};
state_interface_types_ = {"position", "velocity"};
node_ = std::make_shared<rclcpp::Node>( "trajectory_publisher_" );
trajectory_publisher_ = node_->create_publisher<trajectory_msgs::msg::JointTrajectory>(
controller_name_ + "/joint_trajectory", rclcpp::SystemDefaultsQoS( ) );
}
134 void SetUpTrajectoryController( bool use_local_parameters = true )
{
traj_controller_ = std::make_shared<TestableJointTrajectoryController>( );
if ( use_local_parameters )
{
traj_controller_->set_joint_names( joint_names_ );
traj_controller_->set_command_interfaces( command_interface_types_ );
traj_controller_->set_state_interfaces( state_interface_types_ );
}
auto ret = traj_controller_->init( controller_name_ );
if ( ret != controller_interface::return_type::OK )
{
FAIL( );
}
}
150 void SetParameters( )
{
auto node = traj_controller_->get_node( );
const rclcpp::Parameter joint_names_param( "joints", joint_names_ );
const rclcpp::Parameter cmd_interfaces_params( "command_interfaces", command_interface_types_ );
const rclcpp::Parameter state_interfaces_params( "state_interfaces", state_interface_types_ );
node->set_parameters( {joint_names_param, cmd_interfaces_params, state_interfaces_params} );
}
158 void SetPidParameters( )
{
auto node = traj_controller_->get_node( );
for ( size_t i = 0; i < joint_names_.size( ); ++i )
{
const std::string prefix = "gains." + joint_names_[i];
const rclcpp::Parameter k_p( prefix + ".p", 0.0 );
const rclcpp::Parameter k_i( prefix + ".i", 0.0 );
const rclcpp::Parameter k_d( prefix + ".d", 0.0 );
const rclcpp::Parameter i_clamp( prefix + ".i_clamp", 0.0 );
const rclcpp::Parameter ff_velocity_scale( "ff_velocity_scale/" + joint_names_[i], 1.0 );
node->set_parameters( {k_p, k_i, k_d, i_clamp, ff_velocity_scale} );
}
}
174 void SetUpAndActivateTrajectoryController(
bool use_local_parameters = true, const std::vector<rclcpp::Parameter> & parameters = {},
rclcpp::Executor * executor = nullptr, bool separate_cmd_and_state_values = false )
{
SetUpTrajectoryController( use_local_parameters );
for ( const auto & param : parameters )
{
traj_controller_->get_node( )->set_parameter( param );
}
if ( executor )
{
executor->add_node( traj_controller_->get_node( )->get_node_base_interface( ) );
}
// ignore velocity tolerances for this test since they aren't committed in test_robot->write( )
rclcpp::Parameter stopped_velocity_parameters( "constraints.stopped_velocity_tolerance", 0.0 );
traj_controller_->get_node( )->set_parameter( stopped_velocity_parameters );
// set pid parameters before configure
SetPidParameters( );
traj_controller_->get_node( )->configure( );
ActivateTrajectoryController( separate_cmd_and_state_values );
}
200 void ActivateTrajectoryController( bool separate_cmd_and_state_values = false )
{
std::vector<hardware_interface::LoanedCommandInterface> cmd_interfaces;
std::vector<hardware_interface::LoanedStateInterface> state_interfaces;
pos_cmd_interfaces_.reserve( joint_names_.size( ) );
vel_cmd_interfaces_.reserve( joint_names_.size( ) );
acc_cmd_interfaces_.reserve( joint_names_.size( ) );
eff_cmd_interfaces_.reserve( joint_names_.size( ) );
pos_state_interfaces_.reserve( joint_names_.size( ) );
vel_state_interfaces_.reserve( joint_names_.size( ) );
acc_state_interfaces_.reserve( joint_names_.size( ) );
for ( size_t i = 0; i < joint_names_.size( ); ++i )
{
pos_cmd_interfaces_.emplace_back( hardware_interface::CommandInterface(
joint_names_[i], hardware_interface::HW_IF_POSITION, &joint_pos_[i] ) );
vel_cmd_interfaces_.emplace_back( hardware_interface::CommandInterface(
joint_names_[i], hardware_interface::HW_IF_VELOCITY, &joint_vel_[i] ) );
acc_cmd_interfaces_.emplace_back( hardware_interface::CommandInterface(
joint_names_[i], hardware_interface::HW_IF_ACCELERATION, &joint_acc_[i] ) );
eff_cmd_interfaces_.emplace_back( hardware_interface::CommandInterface(
joint_names_[i], hardware_interface::HW_IF_EFFORT, &joint_eff_[i] ) );
pos_state_interfaces_.emplace_back( hardware_interface::StateInterface(
joint_names_[i], hardware_interface::HW_IF_POSITION,
separate_cmd_and_state_values ? &joint_state_pos_[i] : &joint_pos_[i] ) );
vel_state_interfaces_.emplace_back( hardware_interface::StateInterface(
joint_names_[i], hardware_interface::HW_IF_VELOCITY,
separate_cmd_and_state_values ? &joint_state_vel_[i] : &joint_vel_[i] ) );
acc_state_interfaces_.emplace_back( hardware_interface::StateInterface(
joint_names_[i], hardware_interface::HW_IF_ACCELERATION,
separate_cmd_and_state_values ? &joint_state_acc_[i] : &joint_acc_[i] ) );
// Add to export lists and set initial values
cmd_interfaces.emplace_back( pos_cmd_interfaces_.back( ) );
cmd_interfaces.back( ).set_value( INITIAL_POS_JOINTS[i] );
cmd_interfaces.emplace_back( vel_cmd_interfaces_.back( ) );
cmd_interfaces.back( ).set_value( INITIAL_VEL_JOINTS[i] );
cmd_interfaces.emplace_back( acc_cmd_interfaces_.back( ) );
cmd_interfaces.back( ).set_value( INITIAL_ACC_JOINTS[i] );
cmd_interfaces.emplace_back( eff_cmd_interfaces_.back( ) );
cmd_interfaces.back( ).set_value( INITIAL_EFF_JOINTS[i] );
joint_state_pos_[i] = INITIAL_POS_JOINTS[i];
joint_state_vel_[i] = INITIAL_VEL_JOINTS[i];
joint_state_acc_[i] = INITIAL_ACC_JOINTS[i];
state_interfaces.emplace_back( pos_state_interfaces_.back( ) );
state_interfaces.emplace_back( vel_state_interfaces_.back( ) );
state_interfaces.emplace_back( acc_state_interfaces_.back( ) );
}
traj_controller_->assign_interfaces( std::move( cmd_interfaces ), std::move( state_interfaces ) );
traj_controller_->get_node( )->activate( );
}
253 static void TearDownTestCase( ) { rclcpp::shutdown( ); }
255 void subscribeToState( )
{
auto traj_lifecycle_node = traj_controller_->get_node( );
traj_lifecycle_node->set_parameter(
rclcpp::Parameter( "state_publish_rate", static_cast<double>( 100 ) ) );
using control_msgs::msg::JointTrajectoryControllerState;
auto qos = rclcpp::SensorDataQoS( );
// Needed, otherwise spin_some( ) returns only the oldest message in the queue
// I do not understand why spin_some provides only one message
qos.keep_last( 1 );
state_subscriber_ = traj_lifecycle_node->create_subscription<JointTrajectoryControllerState>(
controller_name_ + "/state", qos, [&]( std::shared_ptr<JointTrajectoryControllerState> msg ) {
std::lock_guard<std::mutex> guard( state_mutex_ );
state_msg_ = msg;
} );
}
/// Publish trajectory msgs with multiple points
/**
* delay_btwn_points - delay between each points
* points - vector of trajectories. One point per controlled joint
* joint_names - names of joints, if empty, will use joint_names_ up to the number of provided points
*/
280 void publish(
const builtin_interfaces::msg::Duration & delay_btwn_points,
const std::vector<std::vector<double>> & points, rclcpp::Time start_time = rclcpp::Time( ),
const std::vector<std::string> & joint_names = {},
const std::vector<std::vector<double>> & points_velocities = {} )
{
int wait_count = 0;
const auto topic = trajectory_publisher_->get_topic_name( );
while ( node_->count_subscribers( topic ) == 0 )
{
if ( wait_count >= 5 )
{
auto error_msg = std::string( "publishing to " ) + topic + " but no node subscribes to it";
throw std::runtime_error( error_msg );
}
std::this_thread::sleep_for( std::chrono::milliseconds( 100 ) );
++wait_count;
}
trajectory_msgs::msg::JointTrajectory traj_msg;
if ( joint_names.empty( ) )
{
traj_msg.joint_names = {joint_names_.begin( ), joint_names_.begin( ) + points[0].size( )};
}
else
{
traj_msg.joint_names = joint_names;
}
traj_msg.header.stamp = start_time;
traj_msg.points.resize( points.size( ) );
builtin_interfaces::msg::Duration duration_msg;
duration_msg.sec = delay_btwn_points.sec;
duration_msg.nanosec = delay_btwn_points.nanosec;
rclcpp::Duration duration( duration_msg );
rclcpp::Duration duration_total( duration_msg );
for ( size_t index = 0; index < points.size( ); ++index )
{
traj_msg.points[index].time_from_start = duration_total;
traj_msg.points[index].positions.resize( points[index].size( ) );
for ( size_t j = 0; j < points[index].size( ); ++j )
{
traj_msg.points[index].positions[j] = points[index][j];
}
duration_total = duration_total + duration;
}
for ( size_t index = 0; index < points_velocities.size( ); ++index )
{
traj_msg.points[index].velocities.resize( points_velocities[index].size( ) );
for ( size_t j = 0; j < points_velocities[index].size( ); ++j )
{
traj_msg.points[index].velocities[j] = points_velocities[index][j];
}
}
trajectory_publisher_->publish( traj_msg );
}
341 void updateController( rclcpp::Duration wait_time = rclcpp::Duration::from_seconds( 0.2 ) )
{
const auto start_time = rclcpp::Clock( ).now( );
const auto end_time = start_time + wait_time;
while ( rclcpp::Clock( ).now( ) < end_time )
{
traj_controller_->update( rclcpp::Clock( ).now( ), rclcpp::Clock( ).now( ) - start_time );
}
}
351 void waitAndCompareState(
352 trajectory_msgs::msg::JointTrajectoryPoint expected_actual,
353 trajectory_msgs::msg::JointTrajectoryPoint expected_desired, rclcpp::Executor & executor,
354 rclcpp::Duration controller_wait_time, double allowed_delta )
{
{
std::lock_guard<std::mutex> guard( state_mutex_ );
state_msg_.reset( );
}
traj_controller_->wait_for_trajectory( executor );
updateController( controller_wait_time );
// Spin to receive latest state
executor.spin_some( );
auto state_msg = getState( );
ASSERT_TRUE( state_msg );
for ( size_t i = 0; i < expected_actual.positions.size( ); ++i )
{
SCOPED_TRACE( "Joint " + std::to_string( i ) );
// TODO( anyone ): add checking for velocties and accelerations
if ( traj_controller_->has_position_command_interface( ) )
{
EXPECT_NEAR( expected_actual.positions[i], state_msg->actual.positions[i], allowed_delta );
}
}
for ( size_t i = 0; i < expected_desired.positions.size( ); ++i )
{
SCOPED_TRACE( "Joint " + std::to_string( i ) );
// TODO( anyone ): add checking for velocties and accelerations
if ( traj_controller_->has_position_command_interface( ) )
{
EXPECT_NEAR( expected_desired.positions[i], state_msg->desired.positions[i], allowed_delta );
}
}
}
387 std::shared_ptr<control_msgs::msg::JointTrajectoryControllerState> getState( ) const
{
std::lock_guard<std::mutex> guard( state_mutex_ );
return state_msg_;
}
392 void test_state_publish_rate_target( int target_msg_count );
394 std::string controller_name_;
396 std::vector<std::string> joint_names_;
397 std::vector<std::string> command_interface_types_;
398 std::vector<std::string> state_interface_types_;
400 rclcpp::Node::SharedPtr node_;
401 rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr trajectory_publisher_;
403 std::shared_ptr<TestableJointTrajectoryController> traj_controller_;
404 rclcpp::Subscription<control_msgs::msg::JointTrajectoryControllerState>::SharedPtr
state_subscriber_;
mutable std::mutex state_mutex_;
std::shared_ptr<control_msgs::msg::JointTrajectoryControllerState> state_msg_;
std::vector<double> joint_pos_;
std::vector<double> joint_vel_;
std::vector<double> joint_acc_;
std::vector<double> joint_eff_;
std::vector<double> joint_state_pos_;
std::vector<double> joint_state_vel_;
std::vector<double> joint_state_acc_;
std::vector<hardware_interface::CommandInterface> pos_cmd_interfaces_;
std::vector<hardware_interface::CommandInterface> vel_cmd_interfaces_;
std::vector<hardware_interface::CommandInterface> acc_cmd_interfaces_;
std::vector<hardware_interface::CommandInterface> eff_cmd_interfaces_;
std::vector<hardware_interface::StateInterface> pos_state_interfaces_;
std::vector<hardware_interface::StateInterface> vel_state_interfaces_;
std::vector<hardware_interface::StateInterface> acc_state_interfaces_;
};
// From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest
426 class TrajectoryControllerTestParameterized
427 : public TrajectoryControllerTest,
428 public ::testing::WithParamInterface<
429 std::tuple<std::vector<std::string>, std::vector<std::string>>>
{
public:
virtual void SetUp( )
{
TrajectoryControllerTest::SetUp( );
command_interface_types_ = std::get<0>( GetParam( ) );
state_interface_types_ = std::get<1>( GetParam( ) );
}
static void TearDownTestCase( ) { TrajectoryControllerTest::TearDownTestCase( ); }
};
} // namespace test_trajectory_controllers
#endif // TEST_TRAJECTORY_CONTROLLER_UTILS_HPP_
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef POSITION_CONTROLLERS__JOINT_GROUP_POSITION_CONTROLLER_HPP_
#define POSITION_CONTROLLERS__JOINT_GROUP_POSITION_CONTROLLER_HPP_
#include <string>
#include "forward_command_controller/forward_command_controller.hpp"
#include "position_controllers/visibility_control.h"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
namespace position_controllers
{
/**
* \brief Forward command controller for a set of position controlled joints ( linear or angular ).
*
* This class forwards the commanded positions down to a set of joints.
*
* \param joints Names of the joints to control.
*
* Subscribes to:
* - \b commands ( std_msgs::msg::Float64MultiArray ) : The position commands to apply.
*/
36 class JointGroupPositionController : public forward_command_controller::ForwardCommandController
{
public:
POSITION_CONTROLLERS_PUBLIC
40 JointGroupPositionController( );
POSITION_CONTROLLERS_PUBLIC controller_interface::CallbackReturn on_init( ) override;
};
} // namespace position_controllers
#endif // POSITION_CONTROLLERS__JOINT_GROUP_POSITION_CONTROLLER_HPP_
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/* This header must be included by all rclcpp headers which declare symbols
* which are defined in the rclcpp library. When not building the rclcpp
* library, i.e. when using the headers in other package's code, the contents
* of this header change the visibility of certain symbols which the rclcpp
* library cannot have, but the consuming code must have inorder to link.
*/
#ifndef POSITION_CONTROLLERS__VISIBILITY_CONTROL_H_
#define POSITION_CONTROLLERS__VISIBILITY_CONTROL_H_
// This logic was borrowed ( then namespaced ) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define POSITION_CONTROLLERS_EXPORT __attribute__( ( dllexport ) )
#define POSITION_CONTROLLERS_IMPORT __attribute__( ( dllimport ) )
#else
#define POSITION_CONTROLLERS_EXPORT __declspec( dllexport )
#define POSITION_CONTROLLERS_IMPORT __declspec( dllimport )
#endif
#ifdef POSITION_CONTROLLERS_BUILDING_DLL
#define POSITION_CONTROLLERS_PUBLIC POSITION_CONTROLLERS_EXPORT
#else
#define POSITION_CONTROLLERS_PUBLIC POSITION_CONTROLLERS_IMPORT
#endif
#define POSITION_CONTROLLERS_PUBLIC_TYPE POSITION_CONTROLLERS_PUBLIC
#define POSITION_CONTROLLERS_LOCAL
#else
#define POSITION_CONTROLLERS_EXPORT __attribute__( ( visibility( "default" ) ) )
#define POSITION_CONTROLLERS_IMPORT
#if __GNUC__ >= 4
#define POSITION_CONTROLLERS_PUBLIC __attribute__( ( visibility( "default" ) ) )
#define POSITION_CONTROLLERS_LOCAL __attribute__( ( visibility( "hidden" ) ) )
#else
#define POSITION_CONTROLLERS_PUBLIC
#define POSITION_CONTROLLERS_LOCAL
#endif
#define POSITION_CONTROLLERS_PUBLIC_TYPE
#endif
#endif // POSITION_CONTROLLERS__VISIBILITY_CONTROL_H_
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include "controller_interface/controller_interface.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "position_controllers/joint_group_position_controller.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/parameter.hpp"
namespace position_controllers
{
25 JointGroupPositionController::JointGroupPositionController( )
: forward_command_controller::ForwardCommandController( )
{
interface_name_ = hardware_interface::HW_IF_POSITION;
}
31 controller_interface::CallbackReturn JointGroupPositionController::on_init( )
{
auto ret = forward_command_controller::ForwardCommandController::on_init( );
if ( ret != CallbackReturn::SUCCESS )
{
return ret;
}
try
{
// Explicitly set the interface parameter declared by the forward_command_controller
// to match the value set in the JointGroupPositionController constructor.
get_node( )->set_parameter(
rclcpp::Parameter( "interface_name", hardware_interface::HW_IF_POSITION ) );
}
catch ( const std::exception & e )
{
fprintf( stderr, "Exception thrown during init stage with message: %s \n", e.what( ) );
return CallbackReturn::ERROR;
}
return CallbackReturn::SUCCESS;
}
} // namespace position_controllers
#include "pluginlib/class_list_macros.hpp"
58 PLUGINLIB_EXPORT_CLASS(
position_controllers::JointGroupPositionController, controller_interface::ControllerInterface )
1 // Copyright 2020 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <stddef.h>
#include <functional>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "hardware_interface/loaned_command_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/utilities.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "test_joint_group_position_controller.hpp"
using CallbackReturn = controller_interface::CallbackReturn;
using hardware_interface::LoanedCommandInterface;
namespace
{
35 rclcpp::WaitResultKind wait_for( rclcpp::SubscriptionBase::SharedPtr subscription )
{
rclcpp::WaitSet wait_set;
wait_set.add_subscription( subscription );
const auto timeout = std::chrono::seconds( 10 );
return wait_set.wait( timeout ).kind( );
}
} // namespace
44 void JointGroupPositionControllerTest::SetUpTestCase( ) { rclcpp::init( 0, nullptr ); }
46 void JointGroupPositionControllerTest::TearDownTestCase( ) { rclcpp::shutdown( ); }
48 void JointGroupPositionControllerTest::SetUp( )
{
controller_ = std::make_unique<FriendJointGroupPositionController>( );
}
53 void JointGroupPositionControllerTest::TearDown( ) { controller_.reset( nullptr ); }
55 void JointGroupPositionControllerTest::SetUpController( )
{
const auto result = controller_->init( "test_joint_group_position_controller" );
ASSERT_EQ( result, controller_interface::return_type::OK );
std::vector<LoanedCommandInterface> command_ifs;
command_ifs.emplace_back( joint_1_pos_cmd_ );
command_ifs.emplace_back( joint_2_pos_cmd_ );
command_ifs.emplace_back( joint_3_pos_cmd_ );
controller_->assign_interfaces( std::move( command_ifs ), {} );
}
67 TEST_F( JointGroupPositionControllerTest, JointsParameterNotSet )
{
SetUpController( );
// configure failed, 'joints' parameter not set
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::ERROR );
}
75 TEST_F( JointGroupPositionControllerTest, JointsParameterIsEmpty )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", std::vector<std::string>( )} );
// configure failed, 'joints' is empty
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::ERROR );
}
84 TEST_F( JointGroupPositionControllerTest, ConfigureAndActivateParamsSuccess )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", joint_names_} );
// configure successful
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
ASSERT_EQ( controller_->on_activate( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
}
94 TEST_F( JointGroupPositionControllerTest, ActivateWithWrongJointsNamesFails )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", std::vector<std::string>{"joint1", "joint4"}} );
// activate failed, 'joint4' is not a valid joint name for the hardware
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
ASSERT_EQ( controller_->on_activate( rclcpp_lifecycle::State( ) ), CallbackReturn::ERROR );
controller_->get_node( )->set_parameter( {"joints", std::vector<std::string>{"joint1", "joint2"}} );
// activate failed, 'acceleration' is not a registered interface for `joint1`
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
ASSERT_EQ( controller_->on_activate( rclcpp_lifecycle::State( ) ), CallbackReturn::ERROR );
}
110 TEST_F( JointGroupPositionControllerTest, CommandSuccessTest )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", joint_names_} );
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
// update successful though no command has been send yet
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
// check joint commands are still the default ones
ASSERT_EQ( joint_1_pos_cmd_.get_value( ), 1.1 );
ASSERT_EQ( joint_2_pos_cmd_.get_value( ), 2.1 );
ASSERT_EQ( joint_3_pos_cmd_.get_value( ), 3.1 );
// send command
auto command_ptr = std::make_shared<forward_command_controller::CmdType>( );
command_ptr->data = {10.0, 20.0, 30.0};
controller_->rt_command_ptr_.writeFromNonRT( command_ptr );
// update successful, command received
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
// check joint commands have been modified
ASSERT_EQ( joint_1_pos_cmd_.get_value( ), 10.0 );
ASSERT_EQ( joint_2_pos_cmd_.get_value( ), 20.0 );
ASSERT_EQ( joint_3_pos_cmd_.get_value( ), 30.0 );
}
142 TEST_F( JointGroupPositionControllerTest, WrongCommandCheckTest )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", joint_names_} );
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
// send command with wrong number of joints
auto command_ptr = std::make_shared<forward_command_controller::CmdType>( );
command_ptr->data = {10.0, 20.0};
controller_->rt_command_ptr_.writeFromNonRT( command_ptr );
// update failed, command size does not match number of joints
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::ERROR );
// check joint commands are still the default ones
ASSERT_EQ( joint_1_pos_cmd_.get_value( ), 1.1 );
ASSERT_EQ( joint_2_pos_cmd_.get_value( ), 2.1 );
ASSERT_EQ( joint_3_pos_cmd_.get_value( ), 3.1 );
}
164 TEST_F( JointGroupPositionControllerTest, NoCommandCheckTest )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", joint_names_} );
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
// update successful, no command received yet
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
// check joint commands are still the default ones
ASSERT_EQ( joint_1_pos_cmd_.get_value( ), 1.1 );
ASSERT_EQ( joint_2_pos_cmd_.get_value( ), 2.1 );
ASSERT_EQ( joint_3_pos_cmd_.get_value( ), 3.1 );
}
181 TEST_F( JointGroupPositionControllerTest, CommandCallbackTest )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", joint_names_} );
// default values
ASSERT_EQ( joint_1_pos_cmd_.get_value( ), 1.1 );
ASSERT_EQ( joint_2_pos_cmd_.get_value( ), 2.1 );
ASSERT_EQ( joint_3_pos_cmd_.get_value( ), 3.1 );
auto node_state = controller_->get_node( )->configure( );
ASSERT_EQ( node_state.id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
node_state = controller_->get_node( )->activate( );
ASSERT_EQ( node_state.id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
// send a new command
rclcpp::Node test_node( "test_node" );
auto command_pub = test_node.create_publisher<std_msgs::msg::Float64MultiArray>(
std::string( controller_->get_node( )->get_name( ) ) + "/commands", rclcpp::SystemDefaultsQoS( ) );
std_msgs::msg::Float64MultiArray command_msg;
command_msg.data = {10.0, 20.0, 30.0};
command_pub->publish( command_msg );
// wait for command message to be passed
ASSERT_EQ( wait_for( controller_->joints_command_subscriber_ ), rclcpp::WaitResultKind::Ready );
// process callbacks
rclcpp::spin_some( controller_->get_node( )->get_node_base_interface( ) );
// update successful
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
// check command in handle was set
ASSERT_EQ( joint_1_pos_cmd_.get_value( ), 10.0 );
ASSERT_EQ( joint_2_pos_cmd_.get_value( ), 20.0 );
ASSERT_EQ( joint_3_pos_cmd_.get_value( ), 30.0 );
}
// Copyright 2020 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_
#define TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_
#include <memory>
#include <string>
#include <vector>
#include "gmock/gmock.h"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "position_controllers/joint_group_position_controller.hpp"
using hardware_interface::CommandInterface;
using hardware_interface::HW_IF_POSITION;
// subclassing and friending so we can access member variables
32 class FriendJointGroupPositionController : public position_controllers::JointGroupPositionController
{
34 FRIEND_TEST( JointGroupPositionControllerTest, CommandSuccessTest );
35 FRIEND_TEST( JointGroupPositionControllerTest, WrongCommandCheckTest );
36 FRIEND_TEST( JointGroupPositionControllerTest, CommandCallbackTest );
};
39 class JointGroupPositionControllerTest : public ::testing::Test
{
public:
42 static void SetUpTestCase( );
43 static void TearDownTestCase( );
45 void SetUp( );
46 void TearDown( );
48 void SetUpController( );
49 void SetUpHandles( );
protected:
52 std::unique_ptr<FriendJointGroupPositionController> controller_;
// dummy joint state values used for tests
55 const std::vector<std::string> joint_names_ = {"joint1", "joint2", "joint3"};
56 std::vector<double> joint_commands_ = {1.1, 2.1, 3.1};
CommandInterface joint_1_pos_cmd_{joint_names_[0], HW_IF_POSITION, &joint_commands_[0]};
59 CommandInterface joint_2_pos_cmd_{joint_names_[1], HW_IF_POSITION, &joint_commands_[1]};
CommandInterface joint_3_pos_cmd_{joint_names_[2], HW_IF_POSITION, &joint_commands_[2]};
};
#endif // TEST_JOINT_GROUP_POSITION_CONTROLLER_HPP_
1 // Copyright 2020 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gtest/gtest.h>
#include <memory>
#include "controller_manager/controller_manager.hpp"
#include "hardware_interface/resource_manager.hpp"
#include "rclcpp/executors/single_threaded_executor.hpp"
#include "ros2_control_test_assets/descriptions.hpp"
23 TEST( TestLoadJointGroupPositionController, load_controller )
{
rclcpp::init( 0, nullptr );
std::shared_ptr<rclcpp::Executor> executor =
std::make_shared<rclcpp::executors::SingleThreadedExecutor>( );
controller_manager::ControllerManager cm(
std::make_unique<hardware_interface::ResourceManager>(
ros2_control_test_assets::minimal_robot_urdf ),
executor, "test_controller_manager" );
ASSERT_NO_THROW( cm.load_controller(
"test_joint_group_position_controller", "position_controllers/JointGroupPositionController" ) );
rclcpp::shutdown( );
}
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef VELOCITY_CONTROLLERS__JOINT_GROUP_VELOCITY_CONTROLLER_HPP_
#define VELOCITY_CONTROLLERS__JOINT_GROUP_VELOCITY_CONTROLLER_HPP_
#include <string>
#include "forward_command_controller/forward_command_controller.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "velocity_controllers/visibility_control.h"
namespace velocity_controllers
{
/**
* \brief Forward command controller for a set of velocity controlled joints ( linear or angular ).
*
* This class forwards the commanded velocities down to a set of joints.
*
* \param joints Names of the joints to control.
*
* Subscribes to:
* - \b command ( std_msgs::msg::Float64MultiArray ) : The velocity commands to apply.
*/
36 class JointGroupVelocityController : public forward_command_controller::ForwardCommandController
{
public:
VELOCITY_CONTROLLERS_PUBLIC
40 JointGroupVelocityController( );
VELOCITY_CONTROLLERS_PUBLIC controller_interface::CallbackReturn on_init( ) override;
VELOCITY_CONTROLLERS_PUBLIC
controller_interface::CallbackReturn on_deactivate(
const rclcpp_lifecycle::State & previous_state ) override;
};
} // namespace velocity_controllers
#endif // VELOCITY_CONTROLLERS__JOINT_GROUP_VELOCITY_CONTROLLER_HPP_
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/* This header must be included by all rclcpp headers which declare symbols
* which are defined in the rclcpp library. When not building the rclcpp
* library, i.e. when using the headers in other package's code, the contents
* of this header change the visibility of certain symbols which the rclcpp
* library cannot have, but the consuming code must have inorder to link.
*/
#ifndef VELOCITY_CONTROLLERS__VISIBILITY_CONTROL_H_
#define VELOCITY_CONTROLLERS__VISIBILITY_CONTROL_H_
// This logic was borrowed ( then namespaced ) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define VELOCITY_CONTROLLERS_EXPORT __attribute__( ( dllexport ) )
#define VELOCITY_CONTROLLERS_IMPORT __attribute__( ( dllimport ) )
#else
#define VELOCITY_CONTROLLERS_EXPORT __declspec( dllexport )
#define VELOCITY_CONTROLLERS_IMPORT __declspec( dllimport )
#endif
#ifdef VELOCITY_CONTROLLERS_BUILDING_DLL
#define VELOCITY_CONTROLLERS_PUBLIC VELOCITY_CONTROLLERS_EXPORT
#else
#define VELOCITY_CONTROLLERS_PUBLIC VELOCITY_CONTROLLERS_IMPORT
#endif
#define VELOCITY_CONTROLLERS_PUBLIC_TYPE VELOCITY_CONTROLLERS_PUBLIC
#define VELOCITY_CONTROLLERS_LOCAL
#else
#define VELOCITY_CONTROLLERS_EXPORT __attribute__( ( visibility( "default" ) ) )
#define VELOCITY_CONTROLLERS_IMPORT
#if __GNUC__ >= 4
#define VELOCITY_CONTROLLERS_PUBLIC __attribute__( ( visibility( "default" ) ) )
#define VELOCITY_CONTROLLERS_LOCAL __attribute__( ( visibility( "hidden" ) ) )
#else
#define VELOCITY_CONTROLLERS_PUBLIC
#define VELOCITY_CONTROLLERS_LOCAL
#endif
#define VELOCITY_CONTROLLERS_PUBLIC_TYPE
#endif
#endif // VELOCITY_CONTROLLERS__VISIBILITY_CONTROL_H_
1 // Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <string>
#include "controller_interface/controller_interface.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/parameter.hpp"
#include "velocity_controllers/joint_group_velocity_controller.hpp"
namespace velocity_controllers
{
25 JointGroupVelocityController::JointGroupVelocityController( )
: forward_command_controller::ForwardCommandController( )
{
interface_name_ = hardware_interface::HW_IF_VELOCITY;
}
31 controller_interface::CallbackReturn JointGroupVelocityController::on_init( )
{
auto ret = ForwardCommandController::on_init( );
if ( ret != CallbackReturn::SUCCESS )
{
return ret;
}
try
{
// Explicitly set the interface parameter declared by the forward_command_controller
// to match the value set in the JointGroupVelocityController constructor.
get_node( )->set_parameter(
rclcpp::Parameter( "interface_name", hardware_interface::HW_IF_VELOCITY ) );
}
catch ( const std::exception & e )
{
fprintf( stderr, "Exception thrown during init stage with message: %s \n", e.what( ) );
return CallbackReturn::ERROR;
}
return CallbackReturn::SUCCESS;
}
55 controller_interface::CallbackReturn JointGroupVelocityController::on_deactivate(
56 const rclcpp_lifecycle::State & previous_state )
{
auto ret = ForwardCommandController::on_deactivate( previous_state );
// stop all joints
for ( auto & command_interface : command_interfaces_ )
{
command_interface.set_value( 0.0 );
}
return ret;
}
} // namespace velocity_controllers
#include "pluginlib/class_list_macros.hpp"
73 PLUGINLIB_EXPORT_CLASS(
velocity_controllers::JointGroupVelocityController, controller_interface::ControllerInterface )
1 // Copyright 2020 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <stddef.h>
#include <functional>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "hardware_interface/loaned_command_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/utilities.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "test_joint_group_velocity_controller.hpp"
using CallbackReturn = controller_interface::CallbackReturn;
using hardware_interface::LoanedCommandInterface;
namespace
{
35 rclcpp::WaitResultKind wait_for( rclcpp::SubscriptionBase::SharedPtr subscription )
{
rclcpp::WaitSet wait_set;
wait_set.add_subscription( subscription );
const auto timeout = std::chrono::seconds( 10 );
return wait_set.wait( timeout ).kind( );
}
} // namespace
44 void JointGroupVelocityControllerTest::SetUpTestCase( ) { rclcpp::init( 0, nullptr ); }
46 void JointGroupVelocityControllerTest::TearDownTestCase( ) { rclcpp::shutdown( ); }
48 void JointGroupVelocityControllerTest::SetUp( )
{
controller_ = std::make_unique<FriendJointGroupVelocityController>( );
}
53 void JointGroupVelocityControllerTest::TearDown( ) { controller_.reset( nullptr ); }
55 void JointGroupVelocityControllerTest::SetUpController( )
{
const auto result = controller_->init( "test_joint_group_velocity_controller" );
ASSERT_EQ( result, controller_interface::return_type::OK );
std::vector<LoanedCommandInterface> command_ifs;
command_ifs.emplace_back( joint_1_cmd_ );
command_ifs.emplace_back( joint_2_cmd_ );
command_ifs.emplace_back( joint_3_cmd_ );
controller_->assign_interfaces( std::move( command_ifs ), {} );
}
67 TEST_F( JointGroupVelocityControllerTest, JointsParameterNotSet )
{
SetUpController( );
// configure failed, 'joints' parameter not set
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::ERROR );
}
75 TEST_F( JointGroupVelocityControllerTest, JointsParameterIsEmpty )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", std::vector<std::string>( )} );
// configure failed, 'joints' is empty
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::ERROR );
}
84 TEST_F( JointGroupVelocityControllerTest, ConfigureAndActivateParamsSuccess )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", joint_names_} );
// configure successful
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
ASSERT_EQ( controller_->on_activate( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
}
94 TEST_F( JointGroupVelocityControllerTest, ActivateWithWrongJointsNamesFails )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", std::vector<std::string>{"joint1", "joint4"}} );
// activate failed, 'joint4' is not a valid joint name for the hardware
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
ASSERT_EQ( controller_->on_activate( rclcpp_lifecycle::State( ) ), CallbackReturn::ERROR );
controller_->get_node( )->set_parameter( {"joints", std::vector<std::string>{"joint1", "joint2"}} );
// activate failed, 'acceleration' is not a registered interface for `joint1`
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
ASSERT_EQ( controller_->on_activate( rclcpp_lifecycle::State( ) ), CallbackReturn::ERROR );
}
110 TEST_F( JointGroupVelocityControllerTest, CommandSuccessTest )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", joint_names_} );
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
// update successful though no command has been send yet
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
// check joint commands are still the default ones
ASSERT_EQ( joint_1_cmd_.get_value( ), 1.1 );
ASSERT_EQ( joint_2_cmd_.get_value( ), 2.1 );
ASSERT_EQ( joint_3_cmd_.get_value( ), 3.1 );
// send command
auto command_ptr = std::make_shared<forward_command_controller::CmdType>( );
command_ptr->data = {10.0, 20.0, 30.0};
controller_->rt_command_ptr_.writeFromNonRT( command_ptr );
// update successful, command received
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
// check joint commands have been modified
ASSERT_EQ( joint_1_cmd_.get_value( ), 10.0 );
ASSERT_EQ( joint_2_cmd_.get_value( ), 20.0 );
ASSERT_EQ( joint_3_cmd_.get_value( ), 30.0 );
}
142 TEST_F( JointGroupVelocityControllerTest, WrongCommandCheckTest )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", joint_names_} );
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
// send command with wrong number of joints
auto command_ptr = std::make_shared<forward_command_controller::CmdType>( );
command_ptr->data = {10.0, 20.0};
controller_->rt_command_ptr_.writeFromNonRT( command_ptr );
// update failed, command size does not match number of joints
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::ERROR );
// check joint commands are still the default ones
ASSERT_EQ( joint_1_cmd_.get_value( ), 1.1 );
ASSERT_EQ( joint_2_cmd_.get_value( ), 2.1 );
ASSERT_EQ( joint_3_cmd_.get_value( ), 3.1 );
}
164 TEST_F( JointGroupVelocityControllerTest, NoCommandCheckTest )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", joint_names_} );
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
// update successful, no command received yet
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
// check joint commands are still the default ones
ASSERT_EQ( joint_1_cmd_.get_value( ), 1.1 );
ASSERT_EQ( joint_2_cmd_.get_value( ), 2.1 );
ASSERT_EQ( joint_3_cmd_.get_value( ), 3.1 );
}
181 TEST_F( JointGroupVelocityControllerTest, CommandCallbackTest )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", joint_names_} );
// default values
ASSERT_EQ( joint_1_cmd_.get_value( ), 1.1 );
ASSERT_EQ( joint_2_cmd_.get_value( ), 2.1 );
ASSERT_EQ( joint_3_cmd_.get_value( ), 3.1 );
auto node_state = controller_->get_node( )->configure( );
ASSERT_EQ( node_state.id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE );
node_state = controller_->get_node( )->activate( );
ASSERT_EQ( node_state.id( ), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE );
// send a new command
rclcpp::Node test_node( "test_node" );
auto command_pub = test_node.create_publisher<std_msgs::msg::Float64MultiArray>(
std::string( controller_->get_node( )->get_name( ) ) + "/commands", rclcpp::SystemDefaultsQoS( ) );
std_msgs::msg::Float64MultiArray command_msg;
command_msg.data = {10.0, 20.0, 30.0};
command_pub->publish( command_msg );
// wait for command message to be passed
ASSERT_EQ( wait_for( controller_->joints_command_subscriber_ ), rclcpp::WaitResultKind::Ready );
// process callbacks
rclcpp::spin_some( controller_->get_node( )->get_node_base_interface( ) );
// update successful
ASSERT_EQ(
controller_->update( rclcpp::Time( 0 ), rclcpp::Duration::from_seconds( 0.01 ) ),
controller_interface::return_type::OK );
// check command in handle was set
ASSERT_EQ( joint_1_cmd_.get_value( ), 10.0 );
ASSERT_EQ( joint_2_cmd_.get_value( ), 20.0 );
ASSERT_EQ( joint_3_cmd_.get_value( ), 30.0 );
}
222 TEST_F( JointGroupVelocityControllerTest, StopJointsOnDeactivateTest )
{
SetUpController( );
controller_->get_node( )->set_parameter( {"joints", joint_names_} );
// configure successful
ASSERT_EQ( controller_->on_configure( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
// check joint commands are still the default ones
ASSERT_EQ( joint_1_cmd_.get_value( ), 1.1 );
ASSERT_EQ( joint_2_cmd_.get_value( ), 2.1 );
ASSERT_EQ( joint_3_cmd_.get_value( ), 3.1 );
// stop the controller
ASSERT_EQ( controller_->on_deactivate( rclcpp_lifecycle::State( ) ), CallbackReturn::SUCCESS );
// check joint commands are now zero
ASSERT_EQ( joint_1_cmd_.get_value( ), 0.0 );
ASSERT_EQ( joint_2_cmd_.get_value( ), 0.0 );
ASSERT_EQ( joint_3_cmd_.get_value( ), 0.0 );
}
// Copyright 2020 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_
#define TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_
#include <memory>
#include <string>
#include <vector>
#include "gmock/gmock.h"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "velocity_controllers/joint_group_velocity_controller.hpp"
using hardware_interface::CommandInterface;
using hardware_interface::HW_IF_VELOCITY;
// subclassing and friending so we can access member variables
32 class FriendJointGroupVelocityController : public velocity_controllers::JointGroupVelocityController
{
34 FRIEND_TEST( JointGroupVelocityControllerTest, CommandSuccessTest );
35 FRIEND_TEST( JointGroupVelocityControllerTest, WrongCommandCheckTest );
36 FRIEND_TEST( JointGroupVelocityControllerTest, CommandCallbackTest );
37 FRIEND_TEST( JointGroupVelocityControllerTest, StopJointsOnDeactivateTest );
};
40 class JointGroupVelocityControllerTest : public ::testing::Test
{
public:
43 static void SetUpTestCase( );
44 static void TearDownTestCase( );
46 void SetUp( );
47 void TearDown( );
49 void SetUpController( );
protected:
52 std::unique_ptr<FriendJointGroupVelocityController> controller_;
// dummy joint state values used for tests
55 const std::vector<std::string> joint_names_ = {"joint1", "joint2", "joint3"};
56 std::vector<double> joint_commands_ = {1.1, 2.1, 3.1};
CommandInterface joint_1_cmd_{joint_names_[0], HW_IF_VELOCITY, &joint_commands_[0]};
59 CommandInterface joint_2_cmd_{joint_names_[1], HW_IF_VELOCITY, &joint_commands_[1]};
CommandInterface joint_3_cmd_{joint_names_[2], HW_IF_VELOCITY, &joint_commands_[2]};
};
#endif // TEST_JOINT_GROUP_VELOCITY_CONTROLLER_HPP_
1 // Copyright 2020 PAL Robotics SL.
//
// Licensed under the Apache License, Version 2.0 ( the "License" );
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <gmock/gmock.h>
#include <memory>
#include "controller_manager/controller_manager.hpp"
#include "hardware_interface/resource_manager.hpp"
#include "rclcpp/executors/single_threaded_executor.hpp"
#include "ros2_control_test_assets/descriptions.hpp"
23 TEST( TestLoadJointGroupVelocityController, load_controller )
{
rclcpp::init( 0, nullptr );
std::shared_ptr<rclcpp::Executor> executor =
std::make_shared<rclcpp::executors::SingleThreadedExecutor>( );
controller_manager::ControllerManager cm(
std::make_unique<hardware_interface::ResourceManager>(
ros2_control_test_assets::minimal_robot_urdf ),
executor, "test_controller_manager" );
ASSERT_NO_THROW( cm.load_controller(
"test_joint_group_velocity_controller", "velocity_controllers/JointGroupVelocityController" ) );
rclcpp::shutdown( );
}